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Abstract 



Active stabilization of unstable flight vehicles is an important research area at present. 
It is rarely possible for graduate students to become involved in these research efforts, 
ether cz - largely theoretical basis. For this reason -e chcoc to build « Uuu ve- 
hicle with interesting unstable dynamics. A unicyde robot has been constructed at a 
reasonable cost, which facilitates the evaluation of the control strategies in a laboratory 
environment. 



This research investigates the stabilization of a one wheeled vehicle by means of 
active feedback control. The control methods of a human riding a unicyde are investi- 
gated first and a dynamic model which dosely emulates the process is derived. A one 
wheeled robot with mass and inertia properties similar to those of a young child was 
constructed and used as an experimental vehide for testing various control algorithms. 
The research addresses aspects in the fields of robotics, artificial intelligence and modern 
digital control, but rather than specializing in any of these field*, it strives to combine 
these disciplines in a unique application where the interaction of these fields can be 
studied. An underlying approach of this research was to not only design but also eval- 
uate control system performance in a laboratory environment without incurring large 
financial expenses. The robot has all its electrical and computational power on board, 
with the ability to receive commands from a radio transmitter to change its direction 
and forward speed. A linearized model was derived and optimal control systems to 
stabilize the vehide were designed and simulated. 



VII 



«- - — . ^ „, d ^ „ °™ — * 

The sensing, actaation and control ability of a *r 

P-d with thee of a co mpn texized robot J ^ * ^ ™ 

mpntenzed robot performing a similar task. VvV «. , 

^research and the test vehicle form the has, for theorem an^ell T 

«to the application of nonlinear, robust and adao - ^ 

•-stable system, ^ for 



List 



of Symbols 



^ — yaw angle. 

<t> = roil angle 

8 = pi ten angle 

7 = d + 1; = total angular velocity of the rotary 
turntable T relative to the frame F. 

r*> = constant component of turntable angular velocity. 

7 ~ small perturbation of turntable angular velocity. 

H - total wheel angular velocity about the axle of the wheel. 

ilo = constant component of wheel angular velocity, 

ft = small perturbation of wheel angular velocity. 

p — angle of longitudinal sensor pendulum relative to the frame F. 

<y = angle of lateral sensor pendulum relative to the frame F. 
Wheel mechanical parameters: 

mw = mass of the wheel W. 

rw = radius of the wheel 



f\*$z \t* - moments of inertia of the wheel about 3 mutually 
perpendicular axes originating at the center of mass 
of the wheel I? Is the inertia about the axis along 

ix 



the wheel axle. 
UoicycJe frame mechanical parameter,: 

mF = ma " of ^eframe,F,ofthe tt nicycle 

distance from the wheel «Ie to the center of nm., 
of the nnicyde frame. 

1 - Stance the center of »a» f b located in front 

of the center line of the frame. 
* = distance the center of mass /- is located to the side 

of the center line. 
If, If = moments of inertia of the frame about 3 mutually 
perpendicular axes originating at the center of mas, 
of the frame, if is the inertia about a verical axis, 
«d If about an axis pointing in the direction of normal 
forward motion of the frame 
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Senscr pendulum mechanical parameters: 
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/p = 



spring constant of coil spring attached around the 
pendulum hinge point. 

viscous damping coefficient of the sensor pendulum. 
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if ~ 1^ = moment of inertia of a sensot pendulum about an axis 

passing through its center of mass, and perpendicular 

to the pendulum's plane of oscillation, 
rso = distance from the wheel axle to the hinge point of the 

lateral motion sensor pendulum. 
r$i = distance from the center of mass of the lateral 

pendulum is located in front of the center line of the frame of the unicyde. 
r R3 = distance from the wheel axle to the hinge point of the 

longitudinal motion sensor pendulum. 
r«3 = distance from the center of mass of the longitudinal 

pendulum is located to the side of the center line of 

the frame of the unicyde. 
Other parameters and symbols: 
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fw = viscous friction coefficient at the wheel ?jde where it 
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g = gravity acceleration constant. 

component of the internal reaction force along the un 

unit vector. The W superscript indicates that the reaction 
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The subscript indicates along which unit vector the 

reaction forte is specified. 
Qj — component of the internal reaction moment along the / 2 

unit vector. The meaning of the sub- and superscripts 

are similar to the definition given for reaction forces. 
Qw = control torque applied to the wheel and unicyde frame 
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by the wheel drive motor. 

control torqne appiied to the rotary toratable and 

uaicyde frame by tLe turntable drive motor. 
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Chapter 1 



Introduction 



1.1 Research topic 

The word 'robot 1 was created by the Creek playwright Karel Capek [Capek]. He de- 
rived it from the Czeck words robot*, which means statute labor, and robotnik which is a 
serf [Oxford Dictionary!- In Capek's 1923 play Rossum's Universal Robots, a factory by 
this name creates these mechanical devices that can tirelessly and automatically do the 
work of a man. Since the ~ . rf was coined by Capek, it has found its way into almost 
every other language and robots have beuj:? an almost essential ingredient of every 
modern science fiction writing or movie. In fact, intelligent z-4 capabilities superior to 
those of a human are often attributed to them. As this thesis will demons u*-^ there is 
more to emulating in a robot the relatively basic human skill of riding a unicyde, than 
simply play-writing. 

An important application for feedback control systems is the stabilization of inher- 
ently unstable systems. A human may be regarded as an extraordinary control system 
possessing an impressive array of sensors, actuators, and computational abilities. A 
human riding a unicyde not only stabilizes the unicyde but controls its speed znd di- 
rection. The purpose of this research is to emulate this particular human skill in a robot 
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CHAPTER I. INTRODUCTION 

This research investigate the subiliaticn - m . . . . • 

, JV , maas* - one wheeled vehicle by means of 
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mounted on the vehicle. If the unicycle leaned sideways, a torque was applied to the 
control moment gyro by changing the speed of the wheel that rolls on the ground. These 
investigations dtfer from the approach of this thesis where stabilization by means of a 
control moment gyro was ruled out because it bears no relationship with the method 
employed by humans in stabilizing a unicycle. 

1.2 Organization of the thesis 

This research employs techniques fotnd in mechanical engineering, dynamics, control 
system design, microprocessor technology as well as analog electronics. In order to make 
dear the conceptual contributions of the thesis, the details of the application of these 
disciplines in solving the problem at hand are documented in appendices. The main 
text concentrates on motivating the conceptual principles used in solving the unicycle 
stabilization problem and draws conclusions tss the theoretical and experimental re- 
sults. 

Chapter 2 describes the control methods used by a human riding a unicycle and 
motivates the choice of a dynamic model that represents a reasonable approximation of 
the actual situation. It also addresses the issues considered in the mechanical design of 
the one wheeled robot used in the experiments. 

Chapter 3 describes the design of a control system for the longitudinal stabilization 
of the unicycle robot. The advantages and disadvantages of control algorithms designed 
by successive loop closure and linear quadratic cost function minimization techniques 
are discussed. Experimental results are presented and analysed. It concludes with an 
explanation of why vertical stabilization with accelerometers as the primary sensors 
cannot be obtained in practice. 

Chapter 4 shows that a requirement for stabilization of the pitch instability of the 
unicycle is a sensor which provides a good vertical reference to which the frame attitude 
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Chapter 6 summarizes the main finding, of this research „~ 
this work. ^oi«"« research and proposes extensions of 



Chapter 2 

Dynamic Model for the Unicycle 
Robot 

2.1 Dynamics of a human riding a unicycle 

The process of a human riding a uricyde is quite complex. Not only does a person use 
a multitude of sensory inputs to monitor the process, but the control actions themselves 
are nonlinear. As is the case with most skills learned by a human, many of the control 
feedback loops are closed at a subconcious leveL When we attempt to emulate the pro- 
cess of a human riding a unicycle by a computer stabilized one wheeled robot, we first 
have to simplify the human's actions into mathematical and mechanical models which 
can be simulated a&d implemented in a laboratory. 

A person on a unicycle maintain* longitudinal stability by pedaling faster or slower, 
by leaning his torso forward or backward and by moving his arms forward and back- 
ward. Lateral stability is obtained by leaning his torso sideways, pulling an arm in or 
stretching it out and by steering the wheel into the direction that he is falling by twisting 
motions at the hip joints. Many of these control actions are rather jerky. For example, 
when a person wants to change direction on a unicycle, he would use his torso and out- 
stretched arms as a rev t inn inertia to suddenly twist the lower part of his body and the 
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Observation of a human riding a unicyde shows that lateral stability is obtained 
largely by continuously twisting the wheel in order to steer towards the direction that 
he is falling. The ground reaction at the contact point between the wheel and the ground 
applies a moment to the unicyde which rolls it back to vertical. A turntable mounted 
on top of the robot with its axis of rotation along the ceateriine of the unicyde is used 
to simulate the rider's torso and arms. 

We show in Appendix F that the unicyde can be stabilized by sideways leaning 
aciicsf «!y f but that the yaw angular momentum is uncontroOabie from the lean actu- 
ator. We dedded to exdude the sideways leaning action from the mechanical robot in 
order to simplify the contraction. 

A person uses several sensory systems to monitor the stabilization process while 
riding a unicyde. The four major sensors used to determine orientation with respect 
to vertical are the vestibular system, visual system, proprioceptive sensors and tactile 
sensors [Borah] and (Ormsby]. The vestibular system is the primary orientation system 
of the human and consist* of the semicircular canals (which measure angular vekxity of 
the head) and the otolith organs (whkh respond to linear accelerations and to changes 
in orientation with respect to the gravity vector). Proprioceptive cues are obtained from 
limb position signals and muscle length and tension afferent*, from whkh the brain can 
infer which dynamic forces are acting on the body, based on the person's experience 
in coordination. A person can also determine his orientation with respect to vertical 
from tactile pressure cues on the various parte of his body which are in contact with his 
environment. Finally ey- sight is used, which in combination with a person's experience 
in deducing the direction of vertical from dues in his environment, gives an indication 
of his lateral and longitudinal orientation. All these sensory inputs are combined in the 
human brain to determine the spatial orientation of his body. It is obviously impossible 
to use sensors on the robot with all the sophistication mentioned above. 
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2.2 Mechanical design „f the uaicyde robot 

frame wluch can route aboat the vertical mm. 



The robot was .pecincaUy Signed with ma- and dinte^ona. properties sin^ to 
the* of a human riding on a unicyde. This allowed interring p. fonna.ee compa, 
— . rince the open,oop Un.e coo.taau in the two ca*. WiMBt . ^ ^ 
pension, and weight make the robot ,« tran^ortabfe, it give, the design more 
freedom to select eouipment without being too concerned about physical size. Enough 
batteries could also be mounted on the robot to supply electrical power for extended 
penods of up to two hours of continuous testing without recharging. 

Consider a young person of about SO kg (110 lb) ma*. The mas, can be roughly 
*• body above ^be^thehip. The upper part of the 
body that perform, the twisty motion, wffl be approximated by a 25 kg cylinder of 
0.2 m radius. The moment of inertia about the vertical am. « therefore approximately 

/tor., = -MR 2 = 0.5 kg - :.» 3 

Thi. compare, well with the turntable (battery pack) inertia of r? = 0.5028 kg - m >. 

The rider's leg., -fc. I are usually tfgjiUy bent when resting on the pedals ar* 
approximate by two 12.5 kg cylinder, of r = U cm in radiu, (r) and spaced a distance 
(d) of 0.1 -.apart. The moment of inertia about the vertical centeriine between them 
is given by 

r '<f = ^nr 2 + rruP] = OM kg-m J 
^ Thi, inertia is do* to the robot', frame plus electronic, inertia of if = 04635 kg - 
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The center of mass for the Unicode robot above the wheel axle is given by 
r- = !S££±mL =0 .63 a 
which would be located at approximately hip height for a rider sitting on a unicyde. 

The mechanical parameters of the robot and the measurement methods are given 
in Appendix M. The calculations above do not indnde all the inertia properties of the 
unicyde plus rider, and only serve as an indication that during the robot construction 
an attempt was made to let the mechanical system approximate the real life situation. 

The reader may wonder why the heavy battery pack is placed at the top of the 
onicyde robot. Placing the center of mass high aot only emulates the real case of a 
human on a unicyde, but is also advantageous from a control systems point of view. If 
we consider the unicyde as a simple inverted pendulum, the higher the center of mass is, 
the larger is the fcllndown time constant of the open-loop system. This permits a slower 
sampling frequency in the control microprocessor, so that it has more time to complete 
the calculations of the balance algorithm. Furthermore, during balance recovery actions, 

the wheel drive torque has to accelerate the wheel until its axle is below the center of 

- 

mass of the unicyde. As we show in section LJ2 the effective inertia that the torque has 
to accelerate depends on the height of the center of mass of the superstructure (frame 
^nd turntable). When the center of mass of the superstructure is just above the wheel 
axle height, the control torque has to accelerate almost all of the superstructure's as 
weffias the wheel's inertia during stabilization actions. If, on the other hand, the super- 
structure's center of mass is high above the wheel axle, only the wheel's inertia needs 
to be accelerated (see equations L.4 and L.5). 

The frame of the robot was made out of aluminum. AH of the more complicated 
parts were made on a numerically coded milling machine to provide a light but rigid 
structure to which the motors, gear systems, servo amplifiers, computer rack, sensor 
electronics and the rotary battery pack could be mounted. A drawing of the unicyde 
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robot , shown in Figure 2.1 and more deUiW diawias , « fe 

The rn.croproce.sor rack is mounted ia the ^ of {he ^ ^ ^ ^ ^ 

and the two *rvo amplifier, on either side of it o. the outside of the frame. 

2.3 Dynamics of the unicyde robot 

The unicyde robot M y he modeled a, tn.ee interconnected rigid bodies as shown in the 
schematic diagram of Figure A.I. The various coordinate transformations reosired to 
descnbe the relative angular motions of the three rigid bodies are denned in Figure, A.2 



In Appendix A the dynamic equation, of motion are der:ved by using Newton-Eder 

mechanic Each part of the unicyde is considered a. a f« rigid W; ^ ^ 

forces, control toques and reaction force, and torque, from adjoining bodies acting on 

«■ It abo indude. the dynamic of two sensor penddum. R. and S mounted to the 

^ewychcanfredyswinginthe^^ ^ 

deration, make provision for mounting the sensor pendulums anywhere oa the unicy. 
de frame. 

The conation, are generalized to indude the situation where the center o, mas. of 
the frame is displaced away from the vertical centerline of the robot. Inclusion of the 
product, of inertia into the dynamic equations of motion would significantly compli- 
cate the mathemaiio. Therefore the robot was designed with iU mass distribution a. 
symmetnc^ a. po«ble about the longitudinal and lateral plane, which intersect in 
the vertk^eentedine. This would cause the products of inertia to be neriigiblv small 
or zero (section 7-2, [Greenwood]). It is assumed that the whed roD. on the ground 
wnhout slipping, which place, a nonhofomk constraint on the dynamic equation The 
dynamic equation, are derived for a nonzero nominal turntable speed, which produces 
coupling between the longitudinal and lateral plant dynamic 
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The dynamic equations for the three parts of the unicyde with the two sensor pen* 
dulums are obtained after elininating all the internal reaction forces and moments. It 
results in an eleventh order system with two forcing functions, the wheel torque and the 
turntable torque The equations are summarized in section A. 12 and presented in state 
space form in Appendix D. 

As a means of checking the equations of motion, the derivation was repeated by 
using Lagrange's equations in Appendix B. This method is based on the physical prin- 
ciple that the time rate of change of the scalar generalized momentum at is equal to the 
generalized force Q; due to the applied forces plus an inertal generalized force dee to 
motion in the other generalized coordinates (section [Greenwood]). Often the Q.'s 
are derivable from a potential function, which is the potential energy of the unicyle in 
the gravity field of earth, in the present case. This method avoids the need to elimi- 
aate all the reaction forces as in Appendix A- The no-slip condition on the wheel is a 
zzzhzlczzzz zzzztTZ^zt cq^;cn asd - included LuU> L4gr<uig*» equation as shown in 
section B XL 

In the Lagrange's method derivation, we did not include the sensor pendulums and 
asymmetry of the frame since we only wanted to check the dynamic equations of motion. 
The summary of the dynamic equations of section B.4 indeed confirms that the previous 
equations axe correct. 

A third very useful method for checking the dynamic equations of motion is de- 
scribed in Appendix C. It is based on D'Alembert's principle which states that the 
laws of static equilibrium apply to a dynamical system if the inertial farces, as well as 
the actual external forces, are considered as applied forces acting on the system (sec- 
tion 1-5, [Greenwood]). After determining the D'Alembert forces, D'Aiembert torques 
and gravitational forces on the three parts of the unkyde, the following procedure 
([Bryson 3] provides the dynamic equations of motion: 

• set th two h rizontal components of the moment about the ground c ntact point 
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equal to asro for -Jie whole uakyde. 
. - *i vertical cogent of the moment about * vmicaJ 

zero for the whole unicyde aud indude the appUed ground friction torque. 
• set the ^ent about the a* of the whee, equa, to ,ero for the frame plus 
tumtab.e and indude the applied torque^ and wheel drive frictio* torque. 

•ndude the applied torque <? r and the turutable drive friction torque. 
The resulting equation, in section. C.2 and C.3 serve a. an addition* connnnatio. 
tnat the dynamic model for the system :. derived correctly. 

2-4 Selection of actixators and the drive system 

One of the aim. thi. n^k project «. te beM a robot that can ^ iudf 
t^Toast ^ ^ """^ *~ ™ 



•he former offer, advantage, of a simp* mecWi design ^ noM f . ^ 

MP. of the Proj ec The direct current servo motor. ( (Infraoorl) that were sdected 
have low mertia armature, which aflow, high angular acceleration of the rotor and can 
run at iow^d. with full output torque and uo cogging. A316wattmotorwa.se. 
lected fbr the whee. drive ^tem ^ the robot , ^ „ ^ ^ ^ 

™* ^ at a forward speed a. high as 3 meter, per second (10 hm/h) 
A jailer 151 watt motor w» lected lor the turntaMe drive system. The maximunt 
speed, lor the. motor, are 7000 and 10 000 revolution, per minute for the large and 
smafl motor respectively. 
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A precision cable and poiynrethane chain drive system and aluminum gears were 
used to reduce the motor speeds and increase the motor torques. This system requires 
qo lubrication and the gear ratios can easily be adjusted by using different belt and 
gear sizes. An effort wa« siade to select optimum gear ratios for the drive systems. For 
the wheel drive system it is desireable to select a gear ratio which matches the motor 
rotor inertia to the effective inertia of the longitudinal system dynamics, so that max- 
imum Wheel acceleration can be obtained. This will enable the quickest recovery from 
longitudinal balance disturbances. Appendix K shows how the wheel acceleration as a 
function of the gear ratio can be derived from the dynamic equations of motion. For 
the robot parameters listed in Appendix M, a 24:1 gear reduction is near the optimum 
value for the wheel drive system- 



Lateral system stability is maintained by continuously steering into the direction 
that the unicyde ts falling The optimal gear ratio for the turntable drive system would 
therefore be the ratio that gives maximum yaw acceleration. Appendix G uses the lat- 
eral system dynamic equations of motion and plant parameters to calculate that the 
optimal gear ratio would be approximately 72:1. The practical limitations of the three 
stage gear reduction system that was constructed limited the gear ratio to 60:1, but this 
results in a maximum yaw acceleration that is 93% of the optimum. 



Puke width modulated amplifiers ([Gain Motion]) were used to drive the servo mo- 
tom The particular models that were used modulate the 48 V d.c supply from the 
batteries to supply a regulated current to the motors. They can continuously supply 
10 A to each motor. The servo amplifier receives a reference voltage from the digital- 
to-analog interface card in the on- board microprocessor, which commands a torque in 
the motor. The digital computer interface electronics is described in more detail in 
Appendix L 
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2.5 Selection and construction of scasors 

relative to the frame. The turntable speed aTTl! 
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Analyses in section .3 and Q.3 show that the inverted pendulum modes are observ- 
able from measurement! of accelerometers mounted on the unicyde feme. We decided 
to use this type cf sensor because it would allow the robot to ride on inclined terrain 
and accelerometers of adequate sensitivity could be constructed at an affordable cost. 
Simulations have shown (see later chapters) that the maximum lateral and longitudinal 
accelerations are in the order of 0.1 m/s*. Appendix H describes how servo accelerom- 
eters with an accuracy of about 0.0033 m/s 2 were constructed by careful mechanical, 
optical and electronic design. 

If we are prepared to do tests on horizontal surfaces only, the unicyde can pull a 
light carriage with position sensors to measure the roll and pitch angles directly. We 
constructed such a sensor by means of a small magnet and a Hall effect transistor to 
measure the unicyde frame's pitch angle relative to a horizontal floor surface. This 
provided an inexpensive way to demonstrate experimentally that the unicyde robot 
could be stabilized if the vertical orientation information was available to the control 
system. 



Chapter 3 



Longitudinal System with 
Accelerometer Sensor 

3.1 Introduction 

A significant simplification of the control system design can be obtained if the lateral 
and longitudinal system dynamics can be decoupled. Inspection of the system dynamic 
equations presented at the end of Appendix A, shows *h»* »* is indeed possible to de- 
couple the lateral and longitudinal dynamics if the nominal turntable angular rotation 
speed Tfo is zero and the unicycle frame has left-right symmetry about a vertical plane, 
i.e. r x = r 7 a 0. The first requirement implies that the turntable rotation speeds should 
always be so slow that the gyroscopic effect of this rotating inertia does not significantly 
couple lateral motions into the longitudinal dynamics. Previous research efforts [Iguchi] 
have in fact used a fast rotating gyro on the unicycle to slow down the time constants 
associated with the filling down of the unicycle. Since the purpose of this thesis is to 
emulate the control method of a human riding a unicycle, where this form of gyroscopic 
stabilization is absent, the requirement that ifo must be zero is compatible with the goal 
of this research. 

The second requirement that the center of mass of the unicycle must be on the line 
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The viscous friction constant f v has been ignored since it has no effect on the zero 
locations. 

Since the tachometer reading is 

Vt = « - 9 (3.3) 
we can rewrite equations 3.1 and 3.2 to 

(Ai + AaP+Aa* = JO-Qw (3.4) 
(Aa + = Qw (3.5) 

Take the Laplace transform of the above two equations, eliminate 6(j) and solve 

YM {Iu+*Ix7+Iti)*-J 
Qw(s) ' s[(/„/„ - I* 2 )s> - JI^} 

The zero locations are at 



_ 4../ ~ (mrr + "»irr)g 

" V if + mwrl, + /f + fnr(rw + rr) 2 + /J + m^rnr + it)* 

(3.7) 

If we close a tachometer feedback loop with proportional feedback and command a 
tachometer speed y c 

<?* = *(&-*) (3.8) 

the root loci of two of the closed bop poles approach the zero locations and the other 
dosed loop pole moves to infinity as K becomes large. This implies that with the tight- 
est feedback loop closed on the tachometer measurement, the time constant associated 
with the instability of the longitudinal system can at best be lengthened to that at the 
zero locations. With a tight feedback loop a command of y c = 0 implies no relative 
movement between the wheel and the frame. In this locked wheel situation the unicyde 
fails over more slowly (longer time constant) than the free wheel condition (shorter time 
constant associated with the open loop pole positions). Notice that equation 3.7 gives 



An accelerometer mounted on ti»* n«,wu 

point where it „ locat€d ru ZT ^ , ^ W ^ SPedfiC W * 
ocated. If ontpnt » a ^ ^ by ff j& 

(33) 

*~ - » *- radin. ^ Pftj , ^ ^ 

celerozneter « .ocated. The transfer ^ ^ ^ " ^ ' k < - 

from the wheel torque to the acceferonw.*- ^"nroogn term 

function of the Aerometer heuzht r« n. , on * of 'he other two zeros vary as a 

n «* lt r fO- The locations of these »m • ^ 

y*> ?Uu'» (3.10) 
= -(/ M + /„ls 

*Vnln-rt 2 W-jQ (311) 
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By substitution of the parameters from Appendix M into the equation above we see 
that the pair of zeros are at infinity for the accelerometer mounted at the critical height 
of 

= 0.6675 m ( 3 . 14) 

If an accelerometer were placed *tr« = (r re )«, it would have zero initial response 
to an impulse on the wheel torque. With the accelerometer at this height the di- 
rect feedthrough term fcon the control torqne Qw to the accelerometer output ft be- 
comes zero. It is confirmed by equation N.16 where the coefficient of Q w , namely 
t fgP\ + rwG 7 , vanishes when tr, = (f/e)oo. 

For r/Q < (rjti)^ the accelerometer transfer function has a pair of complex zeros 
and for r^j > (r/tj)oo the zeros are on the positive and uegtfive real axis. The location 
of the zeros is thus very sensitive to small deviations in r/o if r^j is in the vicinity of 

( r fcj)co. 

Another undesireable situation occurs when r/n is such that the accelerometer zeros 
cancel the inverted pendulum poles of the plant These modes then become unob- 
servabie from the accelerometer measurement, which defeats the purpose for which the 
accelerometer was used in the first place. We can solve for this critical height from 
equation 3.12: 

= + (/„/*-/?,), 

JI-zx 

= 0.721 m (3.15) 

The physical explanation, for the existence of this point of unobsexvability is that, if 
the unicycie was allowed to fail over in the plane of the wheel, this point would move 
straight down, Le. it would have no horizontal acceleration. If we think of the spring- 
mass analogy of the accelerometer, it is clear that such an instrument mounted at this 
height, with its sensitive axis pointing along the direction of the unkyde travel, will 
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to the easier* an accderometer is a onnted on tie bob of adm pte p€adaIaiB . 

A method to identify the longitudinal tystem dynamic model experimentally is de- 
scribed in section NJ.2. One of the p roM en. of identify « wUhlt ^ „ ^ ^ 
-nab.,, «> not stay within small potion ^ for very ^ ^ ^ rf 

- tn, case). The — angle. a-nmp.cn i. ba« to deH^ng a vaBd Bne^ixed 
model for the nnicyde dynamic, (Appendix A, B and C). A partial identification of 
the ^tem transfer fnnction, was obtained by hanging the nnicyde upside down by if 
whed. The dynamic model then change, from an invert* to „ ordinary 
to wfcch we can apply sin nsoidal « sign* and meagre freonency ^ponse,. h ^ 
turn the condnsion . drawn that the actnal plant modd i, acceptably doee to the 
theoretical modd became their frean*cv «pons« ^ to whhin 2 dB in gam ami 10 
degree, in pha* over the freonency spectra of signal, ?r «ent in the dynamic system 
doniig typical manueven. 

3.3 Compensator design by successive loop closure 
3.3.1 Nominal design 

Control systems dedgned by sncce«ive loop cWe techniqnc can prodnce compen- 
satom of low order. This i, advantageous when the compensator » implemented in 4 
-croproce-or.wl^uteanmtet^tocnJ^^ 

the dynamic «pon«. Several other consideration, for the sampling rate sdsction a« 
discuMed in section 0.1. 

The longitodinal dynamic are modified to indnde the dday time betw« the in- 
stant that the measurement, are made and the time that the control command is i„ned 
(see section NJ^). 
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Section 0.4 describes the design of the lowest order compensator that stabilizes 
the plant- It consists of an inner loop with proportional feedback of the »»cho!seter 
measurement and an outer loop with a first order compensator on the accelerometer 
feedback signaL Closing a tight tachometer feedback loop first has the advantage of 
decreasing the effect of mechanical nonlineahties in the wheel drive system. The major 
noniinearity is Coulomb friction which could be removed by means of an of&et torque 
which depends on the sign of the wheel sp*ed. Unfortunately the relatively inexpensive 
belt and sprocket drive system used in the robot's wheel drive system also causes time 
varying friction losses which depend on the angular positions of the gears. The most 
effective way of minimizing the effects of these varying torque losses is by closing a tight 
(high gain) tachometer feedback loop first. 

An integral error feedback compensator on the tachometer feedback signal will in- 
crease the low frequency gain in the inner loop and the block diagram of Figure 0.6 
shows how this can be incorporated in the design. For the purpose of finding the mini- 
mum order compensator, however, proportional feedback only will be used. 

The nominal compensator design was obtained through an iterative process and in- 
spection of the root loci and step responses as the loops were dosed and the compensator 
parameters adjusted. The root locus of Figure 0.5 shows that it is theoretically possi- 
ble to find a compensator which stabilizes the unstable modes of the onkyck by using 
an accelerometer censor. The simulation results in Figure 3.1 show that the maximum 
pitch angle from which the longitudinal control system can recover without exceeding 
the maximum available motor torque ((Qw)m*x = 15.12 N - m) is about 5 degrees. 

3*3.2 Robustness of the longitudinal controller 

We have shown in section 3.2 that the zeros of the transfer function to the accelerometer 
output chang e as a function cf the height above the Thed axis These it is uiCunicu. 
The plot of the zero locations in the z-plane (Figure 3.2) shows large changes of the zero 
positions for small variations in the accderometer height, especially in the vicinity of 



■ •»'.»»««fc« < .u«. 4 , , , ^ »S»li>» (actio, of 

K 10 8M P wroogh into the unit circle. 

come, bett* th» , _ 07 J"*" /* ^ b«c*. of the root lod be- 

Placing thm accelsBiB-*— . «~. /■ , 

Eo-^on 3.13 shows " *"* P * 

ra> (r *>- tie acceJeroaeter zeros ^ on the 



3 J. COMPENSATOR DESIGN BY SUCCESSIVE LOOP CLOSURE 25 



real axis. The unstable poie win therefore always be destabilized as it is attracted to 
the zero on the positive real axis for increasing loop gain. 

A schematic diagram which summarizes the finding ! of the longitudinal successive 
loop closure compensator studies is shown in Figure 3.4. Figure 3-5 shows a plot of 
how the dosed loop pole positions change as the acceJerometer height deviates from the 
nominal value of 0.653 m. It shows that the requirement for stability is 

:t'r 

0.54 < fR3 < 0.6707 m 

If the system is expected to be veil damped (closed loop poles within the { = 0.5 curve 
in the z- plane) the range of acceptable r/o values is actually still more restricted. 

3.3^ Experimental Results 

The successive loop closure compensator of section 0.4 was coded in the FORTH com- 
puter lang ua g e and implemented in the on board microprocessor of the unkyde. The 
computer code is given in section P.l. After executing an algorithm for 5 seconds to 
bring the unicyde up to a nominal speed of Qo = 3.0 rad/sec while hand holding the 
robot vertical, the accelerometer feedback loop was dosed. Figure 3.6 shows the two 
measurements during a typical experimental run. The system went unstable when the 
accelerometer feedback loop was dosed and the unkyde released at t s 5 seconds. At- 
tempts to adjust the compensator parameters and accelerometer height experimentally 
to obtain stability were unsuccessful 

Figure 3*7 shows a plot of the accelerometer signal measured during the period be- 
tween 2-5 and 5.0 seconds of the previous experiment. Noise in the signal can be as high 
as 035 m/s 2 peak to peak from one sample to the next. This noise is caused by high 
frequency vibrations of the frame due to uneveness in the wheel drive system and the 
ground surface. 

Figure 3.8 shows the theoretical time response of the accelerometer measurement if 
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3.4 Longitudinal system LQG design 
3.4.1 Motivation for using an LQG compensator 

Compensators consisting of an optimal regulator and estimator have several attractive 
properties which we hoped would address some of the problems encountered with stabi- 
lizing the longitudinal system. First of ail, the design process is highly automated and it 
usually result* in a closed loop system with a good transient response, For the successive 
loop closure compensator design described in the previous section, an iterative process 
was required to obtain a dosed loop system in which all the modes were reasonably 
well damped ({ < 0.5). Even with this simple longitudinal plant model, the choice of 
which loops to close first, the positions of compensator poles, zeros and loop gains is 
a bit of an art. Compensators designed by minimizing linear quadratic cost functions 
will make coordinated use of the measurements and can design state feedback gains for 
closed loop systems that meet reasonable performance requirements. The de*<gner can 
specify the closed loop performance in terms of a cost function which weights the relative 
importance of keeping state errors small and using reasonable amounts of control energy. 

The state estimator design can be optimized if the statistical nature of the process 
and measurement noise is known. Even if these characteristics are known only by ap- 
proximation, the compensators designed by linear quadratic ganssian (LQG) techniques 
provide good noise filtering as well as phase recovery. This is an important improvement 
over the successive loop closure design where ~e would like to filter the noisy acceferom- 
eter signal, but have difficulty in compensating for the phase loss associated with it 

Another advantage is that it is easy to solve the problem of long calculation delays 
in the microprocessor by designing a prediction estimator. It calculates the control 
command at the end of the current sampling period, based on the measurements at the 
beginning of the sample period, allowing a M sample period for the microprocessor to 
perform the calculations. 
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density (R) can U converted to a measurement noise covariance matrix (V) by the 
relationship 

R = 2VT C 

as long as the measurement noise correlation time T c is short compared to the shortest 
time constant of the plant. The shortest time constant for the longitudinal system is 
abont 0.125 sec ( from eigenvalue at s 2 8 r/s) so that a aoise correlation time of T c = 
0.01 sec was assumed. 

The discrete LQG compensator was designed and the closed loop system simulated. 
Figure 0.3 shows the theoretical time responses of the control torque, wheel speed and 
pitch angle during a balance recovery manuever. 

3.4.3 Experimental results and conclusions 

The LQG control system designed in section 0.2 was coded in FORTH after performing 
a transformatioa of the compensator matrices to modal form as described in section O.3. 
Implementing the compensator in modal form reduces the computational load on the 
on-board microprocessor. The FORTH code is shown in section P.2. 

The closed loop system was unstable when tested experimentally. Experimental 
adjustment of LQG compensator gains in order to stabilize the system is unlikely to 
succeed because it has four regulator and six estimator gains which can be adjusted. 

Studies have shown that LQG controllers have serious defects concerning closed-loop 
robustness with respect to plant deviations [Doyle 1). Many techniques are currently 
suggested to improve system robustness [Doyle 2] and could be applied to the unicycle 
stabilization problem However, as we have shown before, the current problems with 
stabilizing the onicyde reside more with the physical problems present when using an 
accelerometer, than with the control system design method. 
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Figure 3.1: Longitudinal System Balance Recovery to an Initial Pitch Angle Error 
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Figure 3 J: Longitudinal Accekrometer Feedback Loop Rootiocos when the Accderom- 
eter axe placed too Low on the Frame 
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Figure 3.5: Rootlocns for the Closed Loop Longitudinal System as a Function of the 
Accekxometer Height above the Wheel Axle 
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Figure 3.7: Typical Longitudinal Acceleration Measurement while the Unicycle is Mov- 
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Figure 3 A The Tune Responses of the Longitudinal Accekraneter Measurement and 
its Components daring a Simulation of a Balance Recovery Sequence 



Chapter 4 



Longitudinal System with 
Vertical Sensor 

4.1 Introduction 

We have shown in the previous chapter that an accelerometer measures not only the 
pitch attitude, but also accelerations proportional to the frame pitch acceleration and 
wheel angular acceleration. These additional terms create zeros in the transfer function 
to the accelerometer output, which makes the control system difficult to design and very 
sensitive to plant parameter changes. In this chapter we will show that a sensor which 
aieasuies the pitch attitude only, will not have these undesireabie zeros. 

4.2 Control system design with pitch sensor 

The compensator designed by successive loop closure is shown in section 0.5. The 
tachometer feedback loop was closed first with a first order integral error compensator. 
This provides a high low-frequency loop gain to decrease the nonlinear effects of the 
drive system. The root locus of this inner loop is shown in Figure 0.9. 

The transfer function to the pitch sensor output has a zero at z = 1 and another 
at 2 = -L78, as shown in the listing of section 0.5.1. The outer loop compensation is 

39 
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3.4.2 LQG design method 

An optimal continuous time regulator was designed first, as described in section N\4. 
We have included integral error feedback of the wheel speed so that we will be able 
to accurately control the unicyde speed by means of an external command. The cost 
function that was minimized to obtain the optimal state feedback gains, contained a 
single factor for the relative weight between the integral error on the wheel speed and 
the amount of control energy used. This weighting factor was varied to obtain a ratio 
which provided good damping on the regulator eigenvalues (Figure HS) and a step re- 
sponse which reached the commanded speed in approximately 5 seconds (Figure N.7). 

Since we did not have an apriori knowledge of the process and measurement noise 
charxteristics, we assumed values for the accefcrometer and tachometer measurement 
noise spectral densities, based on a visual inspection of the relative amount of noise 
present in these two signals during experiments. The process noise spectral density was 
then varied until the eigenvalues of the estimator were in the same s- plane region as the 
regulator eigenvalues. The optimal continuous time estimator gains that were ob taine d 
are shown in section N.6 and the theoretical step response of the dosed loop system 
with noise inputs, is shown in Figure N.1L 

Using the same sampling period as for the successive bop closure compensator, a dis- 
crete equivalent of the continuous time LQG compensator was designed in section 0.2. 
The algorithm by Van Loan [Van Loan] was used to calculate the weighting matrices is 
the discrete time performance index, which will produce the same performance as the 
original continuous time system. 

The discrete time estimator design was performed by first converting the process 
noise spectral density to an equivalen: process noise covariance matrix for the dis- 
crete case, by the duality of the regulator performance index conversion (Appendix 
D of [Bryson 1]}. It is shown in the same reference that a measurement noise spectral 



3.4. LONGITUDINAL SYSTEM LQG DESIGN 

3.4 Longitudinal system LQG design 
3.4.1 Motivation for using «n LQG compensator 

Coc„ co^ of aa optima, ^ Md .^^^ 
-^^-^•oaM^^of^^ enconateredwiZT 

loop closure compensator design described in the 

«s- owjmwi in tne previous section, u iterative r>rrv-«. 

! k ({ * *■ ■* "* ** *- «-* «» * 
"i 0 ^-* •» *- « c 

. muu« pcno<L oased on the measurements at the 

5 * ran sample period for the lwcroprocessor to 

perform the calculations. 



26 CHAPTER 3. LONGITUDINAL SYSTEM V/TTE ACCELEROMETER SENSOR 



the unicvde is initially pitched forward ait an angle of 5 degrees. The maximum mag- 
nitude of the acceleromeier signal a just 0.15 m/j 2 . Plots of the components of the 
accelerometer signal, as expressed in equation 3.9 axe also shown. It shows that while 
some of the components may be reasonably large, they add up in such a way that the 
measured output of the accelerometer sensor is always smalL This causes ihe pitch 
attitude information that is essential for the control system to maintain balance to be 
lost in the noise present in the practical situation. 

It seems logical to attempt to low pass filter the accelerometer signal before it is 
read into the microprocessor. Suppose we add a first order filter ca the accelerometer 
output. This places an additional pole on the positive real z-axis in upper right hand 
diagram of Figure 3.4. The root locus of the closed loop system now has a brunch which 
moves to -oo on the real axis. Without repeating the whole design process here, it can 
be stated that the sensitivity problems discussed in the previous section are aggravated 
by addition of low pass filtering. Physically it means that the phase lag of a low pass 
filter causes the attitude information required by the control system, to arrive later in 
time, In an unstable system it is obviously advantageous to have attitude information 
available to the controller as soon as possible. 

We conclude that, even though accelerometers can in theory be used as attitude sen- 
sors, practical applications are unlikely to succeed. The accelerometers measure both 
the pitch attitude and the frame acceleration at the position where they are mounted. 
The transfer function to the accelerometer output contains zeros whose locations are 
no; only highly sensitive to parameter variations, but also make it difficult to design 
low order compensators which will stabilize the unstable eigenvalues of the plant 

Furthermore, the component of the accelerometer signal which contains the attitude 
information is small (maximum 2 0.15m/ $ 2 = 1.5% of the earth's gravity acceleration). 
It is therefore required that the accelerometers have high sensitivity, but the sensors then 
become susceptible to vibrations of the robot frame as it moves along uneven terrain. 
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r R3 = For example for r^, dunging from only 3,25 csi lower than (r/o)^ (Le. 

• 5%) to 3.25 an higher than (rjQ)^ (Le. + 5%), the zero locations hav. changed all 
the way from z = 1 i j0.24 through z = ±oo to z = 1 ±0.24. 

Since these zeros are points to which two of the root locus branches progress, the 
closed loop system pole locations are also very sensitive to the choice of r/Q. Even if 
r R3 i« known exactly, equation 3.9 shows that the accelerometer signal is a function of 
tjcfi, so that differences between the real plant and the theoretical model which cause 
the actual pitch acceleration (B) to differ more than ±5% from the theoretical pitch 
acceleration, wiii cause the same dramatic shift in the accelerometer zero locations. 

Figure 3.3 illustrates what happens to the root loci if the accelerometer is mounted 
too low on the frame. In this case where r/o = 0.5 m the root loci from the unstable 
region of the z-plane converge toward the two complex zeros outside the unit circle in- 
stead of toward the zero at z = 1 and the compensation zero as in the no minal design 
shown in Figure 0.5. The accelerometer height, r^j, should therefore not be too low 
because it creates a , ha^rier , of zeros which makes it difficult for the root loci from Jhe 
unstable region of the z-plane to slip through into the unit circle. 

If the accelerometer is placed at the nominal design position, r/o = 
0.653 m the system can be stabilized. Inspection of the root locus of Figure 0.5 shows 
that although we may stabilize the system, we will not necessarily obtain satisfactory 
system performance. The damping on the low frequency branches of the root loci be- 
comes better than f = 0.7 at fairly high loop gains(jr tt > 20), at which joint the 
damping on the high frequency branches of the rootlod is rapidly decreasing to unac- 
ceptably low values. 

Placing the accelerometer near (r^)^ = 0.6675 si rcuid net be sensible because of 
the great sensitivity of the accelerometer zero and closed loop pole positions described 
earlier. Equation 3.13 shows that for r/o > (rjtj)^ the accelerometer zeros arc on the 
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These are the two noniolonomic constraint equations for the nnicyde motion. 
Choose the generalized coordinates to describe the position of the unicyde in in- 
ertia! space to be: 
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E: :™z the znicycle is usually close to vertical the coordinate 

4, 0 and their derivatives are small (B.5) 

x t y t ^ f i/ are not necessarily small (B .6) 

Calculate the kinetic and potential energy of the unicyde: 

B.l Wheel 

1. Translation*! kinetic energy of the wheel: 

The position vector from the origin of the inertia! reference frame to the 
center of mass of the wheel is: 

ro = xa x + ya2 + rw&3 

(x + rHrs^^ + ty^rHrs^Jaa + rHrc^aa (BJ) 
r 0 = (z + mrtfotfstf + n^s^c^Ja! 
+ (y - rur^c^c^ + rHTS^S^)02 

- rw0S^G 3 (B-8) 



Appendix B 



EOMs by using Lagrange's 
Method 

Refer to Figures in Appendix A, 

Let (x,y) be the coordinates of the ground contact point P of the wheel, in the 
menial reference frame a,, a,, a 3 . 

Let * be the rotational speed of the wheel about its axle. 
The velocity of the ground contact point P fa then: 



The no slip condition of the rolling of tht-wtad impli^ 

x = vci> = rwibci) 

or rewritten in the differential form; 

luix - r^rctf div = 0 
l.<fy - rwstj) dw = 0 
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(B-2). 

(BJ) 
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inertia matrix 
about center of 
mass at F* = 
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Figure A.6: Unkyde Frame Free Body Diagram 
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Figure A_5: Turntable Free Body Diagram 
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Figw A.1: Unicyde Schematic Diagram 
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lateral State Vector: *,W 

• '. " . • I 

Longitudinal State Vector. [0,fl,p,*,p) r 

Lateral Control Variable: Qt 

longitudinal Control Variable: Q w 

The 7 degrees of freedom are: 6, ft, ,;, a,. 
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Lateral Sensor Pendulum Dynamics: 

A.12.2 Longitudinal system 
Frame Pitch Dynamics: 



= / 3 r '^+/H'n-/^ +(mFrr+mrrT) ^ 

Wheel Rotational Dynamics: 



+ + (m W r + m r + m T )r£ r ]ft 



Longitudinal Sensor Pendulum Dynami 



cs: 



(AM) 



(ASS) 



= -Ur-™,r p9 e-. [kp + mprpg]p - (Ajjj 
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A.12 Summary of the system dynamic equations 

The 30 equations of motion for the 5 rigid bodies contain internal reaction forces 
and momenta, which can be eliminated to obtain the dynamic equations of motion 
for the unicyde 

The algebra for eliminating the internal reaction forces and moments will not be 
shown here. 

The equations of motion for the en: cycle lateral and longitudinal dynamics are: 

A.12.1 Lateral system 

Wheel and Frame Yaw Dynamics: 

- [mFirw + rrfrM + lI? + if + m/{r? + rf)]* 

- m pTFTlQ - m FTWT 7 il 

= -i^Uo* -[/<? + rnfrwr^]^ + /n? 

- ™rr\9<t> - mfr 2 g0 + - Q T (A.90) 
Gyroscopic Coupling Dynamics: 

- [I? + lf + 1? + (m w + m F + m T )rl v + m T r T {2rw + r T ) 
+ mfTjr(2rHr + r F ) + mfv\Y> + mfT X {rw -r Tf)j> + mfnr 2 0 
mp + mr)r^r + (mfrjr + mrrr)r>v]fto^ 

(A91) 

Turntable Rotational Dynamics : 



.AJI. ROTATIONAL EOMS OF LATERAL SENSOR PENDULUM 

A.11 Rotational EOMs of lateral 
A.ll.l Applied moments about 5* 
Refer to Figure A-8 
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sensor pendulum 



1. Reaction, spring and damping momenta: 

^ = -(/^+V)ir+<?fi, + <?|i3 

2. Moment due to reaction forces at hinge point S: 

M = f s ' s xR' 

= r >**(*fix + RZh + Rih) 

A.11.2 Rotational equations of motion of S 



(A.85) 



(A.86) 



-f p cr - v - vRf 



= & 

= ii* 



(AJS7) 
(AM) 
(A.89) 
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A.10.3 Forces acting on S 

1. Gravity force: 

= m p^^i - ^94(3? - <ri 3 ) - m p g(ah - i 3 ) 

= m p ^5i - mpp(^ + <r)52 - mppi 3 ( AJJO) 

2. Reaction forces at S: 

*f3i + A?i* + il3% (A^l) 
A. 10.4 TranslationaJ equations of motion of S 



Jlf + m v gt = m Jr^n + (rsa * i»* - r^] ( A^2) 

+ V + r^j (A^3) 
/if-m^ = mpfr^-r^] (A84) 



■10. TRANSLAHONAL EOiiS OF LATERAL SENSOR PENDULUM 

A.10 TranslationalEOMs of lateral sensor pendulum 

A.10.2 Absolute angular velocity of the i-W 
Refer to Fignre and Figure AJ 



A.10.2 Absolute translation*! acceleration of 5- 

AS *' = 3S + A * s xf* s '+ A * s x(* iS s Xr *s- ) 
i» -a be obtained £ro m eoaation AJ1 by appropriate mhstitat5oa: 



3 s = (rwSl + rne-rsijfa 



A 3 S ' 



+ fcr^ - r<n6]i 3 



{A.74) 



(A.75) 



(A.76) 



(A.77) 



(A.78) 



(A.79) 
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A.9 Rotational EOMs of !ongit. sensor pendulum 
A.9.1 Applied moments about if 

Refer to Figure A.7 

1. Reaction, spring and damping moments : 

Cfr Vfo + Gfo (A.69) 

2. Moment due ;o reaction forces at hinge point fL 

M = r /rR xR R 

= -r?B?h+rrR*ht (A.70) 
A.9.2 Rotational equations of motion of R 



Qi-'p*? = I?i (A.71) 
-fr(>-k,P+r p R* = + # (A.72) 

<?3 = (A.73) 



A.* TRANSLATIONAL SOUS OF L0NG1T. SENSOR PENDULUM 

A V R X( A W R Xf IU1 ') = 0 
4 F = [rwSl + (r* - r p )§ - rftI j - T ^ 

A.8.3 Forces acting on R 

I. Gravity force; 

= -m^/j-*/, +/>] 

= ~"h9{<fa - 6t x - + f 3 ] 

2. Reaction forces at R: 
A.8.4 Translational equations of motion of R 
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A.8 Translation^ EOMs of longit. sensor pendulum 

The dynamic equations of motion of two small passive sensor pendnlnms are in- 
duded in the anicyde model The purpose is to investigate the possibility of 
determining the unicyde attitude information from measurements of the angles 
that the lateral and longitudinal sensor pendulums make with respect to the uni- 
cyde frame. 

A.S.I Absolute angular velocity of tlie r-frame 
Refer to Figure A.7 and Figure A.3. 

It is assumed that the sensor pendulums have negligible mas. and inertia com- 
pared to the rest of the unicyde. In deriving the equations of motion, only forces 
exerted by the unicyde frame on the pendulums are taken into account. Forc« 
and moments exerted by the sensor pendulums on the rest of the unicyde are 
small and are therefore neglected in the analysis. 

A.8.2 Absolute translational acceleration of If 



(A.59) 



S* can be obtained from equation A31 by appropriate substitution. 



+ ( r R24> - Tftltf )f 3 



(A.60) 



w*xr™ = [^ 1+ (« + p)f J + ^,] X (- Pj> f3) 
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A.7.3 Rotational equations of motion of T 

<?f = iff+JW (A.55) 

Qf = iV'-An*. (A456) 

QT-fno-fTl = Iffr + r,) (A.57) 
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A.7 Rotational equations of motion of turntable 
A.7.1 Angular momentum of turntable T 

Refer to Figure A.5 

Absolute angular velocity of the /-frame from equation A.25: 



Angular momentum of T: 



where: 



B = Bih + B 7 h + B3J3 



Bi = I** 
B, = jf 8 

Note : j} a measured in relation to frame F 



(A.49) 



(A.50) 



M = B =(E) r + A w F xB 



(A-51) 



(B) r = /f Vix + ll»h + / 3 T (* + rj)h 



(A.52) 



x H = 



= /W/i-/fW/2 (A53) 



A.7.2 Applied moments to T 

Qifi+Qlf2 + (QT-fTi»h 



(AM) 



A.6. TRANSLATIONAL EQUATIONS OF MOTION OF TURNTABLE n 

A.6 Translation*! equations of motion of turntable 
A.8.1 Forces acting on T 

Refer to Figure A.5 



(A.43) 



1. Reaction ibrcea from frame, F: 

2. gravity : 

+ mrjij = -mrgOA - m T ^f 2 _ mT9 f 3 (jL44) 
A.6.2 Absolute acceleration of f 
The acceleration of I* can be obtained from 



r x = r, = 0 



equation A.31 by letting r r - ^ 

s r, -(nra+f^)/; +lPlrlW _ (nr+Pr) ^ (jL45) 

A.6.3 IVanslationaJ equations of motion of T 



R\ +m T g9 
R% — m T g<j> 
Rj - m T g 



= 0 



(A.46) 
(A.47) 
(A.48) 



APPENDIX A EOMS BY NEWTON-EULER MECHANICS 



(A-41) 



- riR" + *iRTe + n R? - rjiif 



(A.42) 



(A-39) 



A.5. ROTATIONAL EQUATIONS OF MOTION OF FRAME F 

.+ H'F-r T )RT- ri RT)j 2 

A.5.2 Rotational equations of motion for F 

from equation AJ25 : 

Using Euler's equations of motion: 

Mi = /,tp,+(/ 3 

JW3 = / 3 U>j + (/,-/,)«,,„, 

where: 

■M. = applied moments 
w% = ^ and ipi = ^, 
wj - 0 and tiij = 0, 
«»3 = 0 and w = # 

Note that afl products w,-w } a 0 in Euler's Equation. 
A.5.3 Rotational equations of motion of frame F 



(A.40) 
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A.5 Rotational equations of motion of frame F 

A.S.I Applied moments about F* 

Refer to Figure A.6 

1. From wheel; 

= -Cft/i + 9'h) - [Qw - fwifl - 9)]'h - Q?(-9fi + h) 
= (Qf* - Q?)fx +fw(to + fw<l ~ fw6 - Qw)f 7 ~ + Qz)h 

(A-35) 

i 

2. From turntable: 

-QTh^h-(QT-fTn)h = -Qtfi-QlhHfTr*+fTn-QT)h (AJ6) 

3. Due to reaction forces at 0: 

A? = r^xF 

A ft A 

= -n -r 2 -r F 

= {r 7 R?B + T*R? -r F B?]h 

+ [rpRr-rpRfe-r^e-r^h 

+ hiT-^r+r^l/a (AJ7) 

4. Due to reaction forces at T: 

M = ^xF 

/i h h 

= ~ r i -r 2 (it - r F ) 
-Rj -B$ -Rl 



A.4. TRANSLATIONS EQUATIONS OF MOTION OF FRAME 
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-Of* : 

r = n/i + r,/, + r F f 3 



(A^7) 



/i h h 

I 9 i, 
n r a rjr 



fA.28) 



, « r x(^^).0h«M» ki 



involves prodoc* of small angular rata 

(A.29) 



' =( " fl+ ^-^^^-(-^^^l/ J+ (^- n ,V3 (A.31) 
TWuional equations of motion of frame F 



-Z?'-R?-RZ-m F9 = 



'Mneft+rrf-r,*) (A32) 

(AJ3) 
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AA Transiational equations of motion of frame 

A.4.1 Forces acting on F 
Refer to Figure A.6 

1. Reaction forces from wheel,W: 

2. Reaction forces from turntable,! 1 : 

-Rlh-B*h-Rlh 

3 Gravity: 

— mjrgaz — —mpg(4v)2 + W3) 

= -m F g[4>f 7 - 9 f x + f z ) 
- mpgOfx - mrg<j>f 7 - rrtfgfz 

A.4.2 Absolute transiational acceleration of F* 

Refering to Figure A~2, the absolute angular velocity of the F frame is: 

A w F Z4>f l +9f 2 + j>h (A-25) 
The acceleration of the center of mass of the onicyck frame, F, is: 

a^^oo^^ xf° r +Vx(Vxf° r ) (A.26) 

where cto is known from equation A.5 



(A-22) 
(A.23) 



(A-24) 
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A. 3.3 Angular momentum of the wheel 

the angular momentum of the wheel is: 

3 = H\i>i + &2*>i + 

= fr^i + irCflo + n^ + ^^j (A.15) 

Since the component* of the angular velocity are taken along the principal axes of 
the wheel, the angular momentum is given by: 

Hi = lf Vi 

Wi = component of wheel speed 

ft = (fto + ft)= wheel rotational speed 

Using Eulex's Law for rotational motion of rigid bodies, equate the total applied 
moment about the center of mass of the wheel to the change of angular momentum: 

M = M ^Mr+A*" x M (A.16) 

ir = i?*** + /fn*, + jf (a.17) 



A <B W xH - 



* 0 4> 

i?4 /T(fto+n) jf* 



= -/^flo^i + if^o^ (A.18) 



A J. ROTATIONAL EQUATIONS OF MOTION OF WHEEL: 

A.3 Rotational equations of motion of wheel: 
A.3.1 Applied moments about O 
Refering to Figure A.4: 



1- The applied moments at the center of mass are: 

Q? * + [Qw - fw(n - *))*, + (Q~ _ fcifa (AJ2) 



where: 

Qw : wheel drive torque 

fw : viscous wheel axle friction 

/c : assumed viscous friction at ground 

2. Moments due to reaction forces at ground contact P: 
A? = fO'xjP' 

= +r w R%v>i - rwRi wj 

A.3.2 Absolute angular velocity of tbe w-frame 



= M+^s^wj + c^wa) 



(A.13) 



(A-14) 
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to X v = 4 Q $ 

rw(fto + ft) -rwi 0 



So - rwtlibi + rw (*W - tbj 



A.2.2 Forces acting on W 

Refer to Figure A.4 for the wheel freebody diagram: 



1. Reaction forces at 0 : 



2. Reaction forces at P : 



3. Gravity 



(A-5) 



IA.6) 
(A.7) 
<AJ) 



A.2.3 Translational equations of motion of wheel 

By application of Newton's lawB of motion, the three components of the forces 
acting on the wheel are equated to the wheel mass multiplied by the appropriate 
acceleration component: 



It? 4" — ffly^ 

R$ + R^ — m^g 



0 



<AA) 
(A.10) 
(A.11) 
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A.2 ^Variational equation, of motion of wheel 
A.2.1 tiowMcderaUon of point O 

» i*. A. 2 ^ ^ „ fc ^ ■ 

Linearization assumptions made throughout the analysis are: 

1. Angles and perturbation angular rates an. «n,n 

fio respectfully. Ma 

2. sin ^ 3f ^ and C08 ^ a, j 

3- Products of small ang.es and angular rates are negligee. 
4. ft s jlo + u 
where: 

«o = constant component of wheel anguUr velocity; it can be large. 
0 = smaD perturbation of wheel angular velocity, 
with these assumptions, 3 can be approximated by: 

^*t& 1 + (n 0 +ft)* 2+v tf 3 

Assuming no slip at the ground contact ooint P tu , . 

^ the wheel, 0, is: ^ ^ P « ^ ^ of the center of mass 

acceleration of 0 is : 

*° = (5)r+uTxu 
. _ (AJ) 

* = angular velocity of w -frame 



Appendix A 

EOMs by Newton-Euler 
Mechanics 

A.l Definition of variables 

In this appendix we wOl derive the dynamic equations of motion (abbreviated 
EOMs) [for the anicyde robot] by using Newton-Euler mechanics. A schematic 
diagram of the anicyde model is shown in Figure A.l and the coordinate frames 
for the various part* are given in Figures AJ2 and A~8. The symbols used in the 
t.iesis are given in the table at the beginning of this document. 
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» «-* o» „ ^ mM 
TU „ ^ „ ^ ^ owJtitBde of Mnsoty inpito 
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obtain the pitch angle The meaaurement of the original accderometer signal 
can then be filtered with a long time constant to correct for drift errors. 

This so called complimentary filtering technique is essentially the same as that 
used in a vertical gyro. A rate gyro which measures the pitch rate, combined 
with the current accderometer, will probably be a cheaper solution to the 
sensor problem than using a vertical gyro. 

- Stabilize near zero speed 

The lateral motions are uncontrollable at zero speed by twisting the wheel 
through small angles. Preliminary simulations have shown that the unicy- 
de can be stabilized near zero speed if it is commanded to perform oiaaS 
forward-backward oscillations about a fixed point This conjecture should be 
investigated because it can produce interesting results cc controlling a dy- 
namic system which oscillates between a controllable and an uncontrollable 
state. An alternative way of controlling the unicyde at low speeds, would be 
by turning the wheel into the roil direction and then using the longitudinal 
control system to erect the unicyde. 

- Nonlinear lateral control system 

The twisting motions performed by a unicyde rider's body at very low speeds 
are large amplitude ( ± 30 degrees), which leads us to expect that a nonlinear 
on-off type controller on the turntable torque might provide stabilization. 

6.3 Conclusion 

The investigation into computer stabilization of a unicyde robot has yielded valu- 
able insights into the dynamics, control and sensing aspects involved in the process. 

A major part of our time resources had to be devoted to selecting the equipment, 
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- Lateral control system design 

Chapter 5 points out that if « accekrometer i, used a. tie lateral attitude 
sensor, similar problem, exist in designing a robust coutrol as in the 

longitndinal case. An LQG controller using a measurement of the tenure 
angular velocity and the name rofl angle, was proposed. Weshowed that this 
controller would provide acceptable performance for wheel speeds in the range 
of approximately ±5 % of the nominal design speed and that compensator 
gain scheduling should be used if the unkycle is to be operated over a wide 
speed range. The design was not tested experimentally, but we expect it to 
perform satisfactorily if good roil attitude information is available and an 
effective gain scheduling scheme is implemented. 

6.2 Recommendations for future research 

- Test lateral control system 

A rcT -^1= cc^cr = iig a trailer wheel which extends sideways from the 
frame should be construct*. It can be used to test a lateral stabilization 
algorithm using linear control at a conrtant wheel speed and on a horizontal 
noor. A gain scheduling algorithm can be developed to control the robot 
over a range of wheel speeds for which the lateral plant dynamics change 
significantly. 

- Inertia! vertical sensor or additional sensors 

A vertical gyro should be mounted on the unicyde robot, so that frame roll 
and pitch angle, relative to vertical can be measured. This would permit the 
vehicle to travel on surfaces which are inclined and rough. 

It is possible that another non ground contact sensor in addition to the single 
accekrometer, might relieve the sensitivity of the closed loop system design. 
If we have a measurement of the pitch rate or pitch acceleration, we can per- 
form a single or double integration on these measurements respectively, to 
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Candidate longitudinal control sytems were designed using an accelerometer 
as the balance sensor. Since these sensors do not measure the attitude alone, 
(see section above), the pitch attitude information required by the control 
system, depends heavily on the accuracy of the plant dynamic modeL We 
have shown that the transfer function to the accelerometer output has zero 
which make it difficult to design a robust control system. These zeros can be 
relocated to more convenient positions in the a- plane by changing the location 
of the accelerometer on the robot frame, but the resulting zero positions are 
extremely sensitive to small changes in the sensor location. The maximum 
acceleration of the unicyde frame a on the order of only 3% of gravity ac- 
celeration daring typical manuevers. The accelerometers were designed with 
adequate resolution to measure signals in this range, but during experimen- 
tal tests it was found that frame vibration noise due to the roughness of the 
terrain over which the robot traveled was of the same order of magnitude as 
the desired signaL 

For these reasons above, none of the candidate control systems described in 
Chapter 3 could stabilize the unicyde robot by means of a single •tuaexuiu- 
eter as the main balance sensor. An important contribution of this research, 
however, is the investigation of the theoretical and experimental reasons why 
accelerometers have limited capabilities as attitude sensou. 

Chapter 4 shows that if a verted sensor is available, a robust longitudinal 
control system can be designed. The control system is based on a direct 
measurement of the kinematic variable which ind : cates the attitude, instead 
of relying on an indirect measurement and the accuracy of the plant model 
By performing experiments on a horizontal surface and measuring the robot 
frame angle with respect to the ground, we experimentally demonstrated 
the performance of a robust control system with good stiffness to stabilize a 
unicyde longitudinally. 
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wheel toique ha. three component,: 

1- The forward acceleration of the center of thewheeL 

2- The oadcward accefcratioa of the point on the W .here the ac- 
ceierometer is mounted. 

of the frame froin verticaL 
. . ^««ice oetween the two large signals 

- «ta „ a. I1Ml! ^ ^ ^ 

*«nal) to aertirf loeatio. the " 

Optical or inertia] season videb cooM be mounted on the enicyde to pfo- 
^^^^•'^^-"^""^^forth. 
^ **. n^^tae^. ^ ^ ^ 

smooth horizontal floor surfaces. 

Longitudinal control syrtem design and test. 
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unicycle. The result is a robot with interesting unstable open loop dynamics, 
which can serve as a test bed for modem control system techniques. 

- Emulation of a human riding a unicycle 

The actuation methods and sensor information used by a human riding a uni- 
cyde were evaluated in Chapter 2. Observations of a human's control actions 
and calculations of a person's mass and inertia properties we*« u>iauu»ed to 
propose a mechanical robot configuration which could emulate the impor- 
tant features. Wc constructed such a one wheeled robot with an on- board 
power supply, actuators, sensors and microprocessors to perform the active 
stabilization task. 

- Dynamic model tor a unicycle robot 

The linearized dynamic equations of motion that describe the lateral and 
longitudinal model of a one wheeled robot were derived. It was shown that 
lu« l+itual *uu totigiiuuiiui dynamics decoupk ender certain reasonable con- 
ditions and that the lateral dynamics vary with the unicycle speed due to 
the gyroscopic effect of the rotating wheeL Physical explanations for the dy- 
namic system modes and the zeros of transfer functions from the actuators 
to the various sensors, were presented. 

- Evaluation of balance sensors 

We tried first to use accekrometers on the unicycle robot to determine the 
deviation of the frame from vertical. This choice was motivated by the desire 
to emulate the balance sensing capability of the human ear, and on consid- 
erations of cost. Furthermore, theoretical analysis showed that we should 
be able to determine the vertical directl/ by placing the accelerometer at a 
particular vertical location. 

However, in practice, we wera nnahle to stabilize the longitudinal motions 
of the unicycle using an accelerometer. We believe the reason for this is as 
follows. The longitudinal accelerometer signal in response to an impulse in 
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recordation, for future search I,,, ^ ve 
6.1 Contributions 

- Research topic 
The automatic stabilization of aMJcy de^ made ^ by ^ rf 
a -que ccmbLution of state of the art computer hardwa* Md modem 
control sy«ema aaal^ and design tods. Since this research commenced 
several sources noticed that the idea of a computer stabilized nnicyde 
has appeared dsewhere. Only one reference of a practical appfication =ou,d 
be found in the literature (Iguchi], but the control method there does not 
bear any relationship to the way a human controls a nnicyde. Using new 
compact, and powerful microprocessors, the research in this thesis resulted' 
- a one wheeled rob. with all its computational and electrical power on 
board, to emulate the longitudinal stabilization used by a human riding a 
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R0LL ME AS. GAINS VERSUS WHEEL SPEED 
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Figure 5.5: Lateral System Optimal Estimator Gains on the Roll Angle Measurement 
as a Function of taeWieel Speed ™a»wwneim 
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REGULATOR GAINS VERSUS. WHEEL SI^EED 
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Figure 5.3: Lateral System Optimal Regulator Gaias as a Function of the Wheel Speed 
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LATERAL C.L. P0LES VERSUS WHEEL 
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_for Wheel Speed = 3 rad/ se 
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Figure 5.2: Lateral System Cloned Lo«n p-j 

LQG compensator * * Whed Speed for 
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Figure 5.1: Dependence of the Lateral Plant Eigenvalues on the Wheel Speed, fio 
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S3 

speed,. At sen, wheel .peed, the roll ph. become infinitely large, since it b ob . 
viondy impossible to stab** the unieyde robot by toting the frame abont the 
vertical a*., j, ^ ^ 

that the robot is falling and then udng the longitndinaj centre, system to erect 
the unkyde, can work at low wheel speeds. 

Plot, of the estimator gains on the tachometer and roil angle mea^rement, are 
shown in Fignres 5.4 and 5.5 respectively. It is evident that gain schednling on the 
estimator gains will afco be required as the wheel speed changes. It i, interesting 
to note that none of the estimator gain, become lively large at any speed as 
the regulator gains did. Abo notice that at zero wheel speed, mea^rement of the 
turntable tachometer speed , i, n*d to improve the estimate of the yaw rate and 
turntable speed only (Llat(U) = Llat(M) = „ ) while the measurement of the 
roD angle sen*, is used to improve the roll angfe and roll rate «timate (Llat(24, 
= Llat(W) = 0). * 

From the investigation, in this chapter it is condnded that the lateral system 
can be stab*** at nonzero wheel speed, by twining the turntable. Appropriate 
sensors wouKl be a tachometer to measure the turntable speed and a roD angle 
senso. If U* unicyde is to operate over a wide range of forward speeds, some 
form of scheduling of the compensator gains with wheel speed wfll be required 
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the amount of control torque used. The ratio of these two weighting factors 
varied to obtain a dosed loop response which would let the yaw rate reach the 
commanded value in approximately 5 seconds (Figure Q.2). 

A full order state estimator was designed by assuming the same noise spectral 
densities as for the longitudinal system design, Section Q.4 shows the optimal 
regulator and estimator gains as well as the regulator and estimator poles. 

7h» Hosign above was for a nominal wheel speed of fto = 3 rad/sec AU the 
dosed loop poles are well damped (f > 0.7) and the real parts of the estijuiator 
eigenvalues are comparable to those of the regulator eigenvalues. If we use the 
compensator gains for the nominal wheel speed of 3 rad/sec, but vary the wheel 
speed, the dosed loop eigenvalues of the system change as shown in Figure 5.2. 
Since the lateral plant eigenvalues change as a function of wheel speed, the dosed 
loop system will remain stable for 

2.7<fto< 3.6 rad/sec 

The range for acceptable closed loop response is probably even smaller ( in the 
order of flo± 5%) so that gain scheduling will be required for the lateral control 
system. If we use the same cost function, but the different lateral plant models 
for the range of possible wheel speeds, the optimal regulator f ee dback gains as a 
function of the wheel speed, can be calculated. The regulator gain on the integral 
error of the yaw rate (c) and the feedback gain on the turntable speed (17) remain 
constant for all wheel speed values. Figure 5.3 shows how the other regulator gains 
should be varied to minimize the ccet function at different wheel speeds. The yaw 
rate feedback gain remains relatively constant, but the feedback gains on the roll 
angle and rate changes significantly, especially at lower wheel speeds. 

It is r id en t that if some form of a compensator gain scheduling technique is used, 
the regulator gains will have to be changed rapidly with wheel speed at low wheel 
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Two of the accdcrometer transfer function zeros art at 
. _ , / 9J72 - rw Jii 

The third acceleroineter zero at s = 0 does not appear in equation 5.12 because 
it is cancelled by the pole at s = 0 due to the zero turntable friction assumption. 
Equation 5.13 pKrms that the locations of the accekrometer zeros are independent 
of the wheel speed ft©- 

By substitution of the parameters from appendix M into equation 5.13 we see thai 
the pair of zeros are ? * infinity for the accelerometer mounted at the critical height 
of 

(r*U = (/ai 

= 0.695 m (5.14) 

For rs3 < (rs2)oo the accelerometer transfer function has a pair of complex ze- 
ros on the imaginary axis. An impulse torque on the turntable motor will cause 
the frame acceleration to inaease initially and then recede back to zero. For 
r 53 > (rs3)» the accelerometer zeros are on the positive and negative real axis. 
The frame acceleration at this height win therefore have a nonminimum phase 
behaviour to an impulsive turntable torque The physical meaning of the zeros at 
± joo when rsz = (^53)00 « therefore that the frame will have no initial accelera- 
tion at this height, for an impulsive control torque. The point on the frame acts 
as a virtual pivot point during motions, with points above and below (r S3 ) 00 on 
the frame accelerating in opposite directions. 

Theoretically the best place to locate the accelerometer would be at (r S3 ) 00 . From 
the print-out in section Q .3.1 we notice that, for a nominal wheel speed of fto = 3 
rad/sec, the lateral plant eigenvalues are at approximately 3.29 rad/sec. We can 
use equation 5.13 to solve for the height (r^w, when the accelerometer zeros 
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the tnaHferfa nc tio M to the roD angle and yaw me: " 



^-^^tArf-J^W" (5-4) 



where 



Qt(s) ^-JuKAs + f G )-j vJul33 ; (5.5) 



* = /» + /» 
From equation Q.18 the accelerometer measurement is 

*(*) = + rs3 )i + r^flo^ + ^ 
Taking the Laplace transform, 5.7 becomes: 



(5.6) 



(5.7) 



Substitute equation, 5.4 and 5.5 into 5.8 to obtain the transfer function: 



if define 



J„-l? + mw S w x ^^(^ + r F ) + mTTw{rw + rr) (510) 
we notice from equation Q.13 that 



and the accelerometer transfer function can be rewritten as 



(5-11) 
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unicyde yaw modes in the measurement of the turntable speed relative to tic 
frame 

The transfer function to the accelerometer measurement has three zeros: one at 
s = 0 and two at ± 1.0284 rad/sec on the imaginary axis of the s-plane The 
position of the two zeros whkh are not at the origin, is a function of the height 
r S3 of the accelerometer above the wheel axk, but not a function of the wheel 
speed, as we will show later. 

If an accelerometer is used as a balance sensor the control system designer is con- 
fronted with the same problems encountered in the longitudinal control system 
design. First of all, the three zeros on the imaginary axis cause a 'barrier' of zeros 
which makes it difficult to design a compensator which will bring the root loci 
from the unstable poles in the right half of the s-plane to the left half plane 

We will now proceed to calculate the accelerometer zero positions analytically and 
show that their locations are very sensitive to changes in r$3 and plant parameter 
changes: 

The lateral plant dynamics from equation Q.6 are: 

Aaf + ib* = Jn4>-f<rt-QT f5.1) 
In4> = Ja*'+/M* (5.2) 
/32<S + /»* = Q T (5.3) 

where the coefficients of the state variables are denned in equations Q.7 through Q.14. 
The viscous friction constant fx has been i gn ored in the eq uati ons above since it 
has no effect on the zero locations. 
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intend to control these variables. 

At nonzero wheel speeos, the gyroscopic effect of the wheel change, the eigenvalue, 
Of the inverted pendulam mode.. Figure 5.1 .how, that, „ the wheel .peed incre^e. 

« the robot can reach a maxim** rotor .peed of 7000 revolution, per minute. W.th 
the 24:1 reduction gear .yutem, this entail, a maximum wheel .peed of approximately 
30 rad/.ec. Figure 5.1 .how, that at this .peed the inverted pendulum mode, have 
changed from real eigenvalue, to a pair of .table complex eigenvalue, in the ..plane. It 
is evident that the lateral control syutem will have to adju.t its compensator gain, a, 
the wheel speed changes. 

The system cnamtemtic. listed in section Qj.i « for a Domiaai ^ ^ rf 
fio = 3 rad/sec. The controllability vector CTR .how. that afl mode, are control 
from the turntable torque. 

The output distribution matrix i. ,hown for three possible sensors: 
• a tachometer measuring the turntable speed relative to the frame 
. an accelerometer mounted on the frame with its seaside ax» in the lateral direc- 



tion 



. » roD angle .ensor, which could consist of a vertical gyro or a device for measuring 
the frame angle relative to a horizontal surface, as described for the longitudinal 
system in the previous chapter. 

The transfer function from the turntable torque to the tachometer output shows 
that three of the four eigenvalues are nearly cancelled by zeros and that conse- 
quently only the residue of the mode at s = 0.238 rad/sec is not small This makes 
phys,cal sense since we cannot expect to see much of the inverted pendulum and 
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5.1 Introduction 

In this chapter the lateral system transfer functions for different sensors at different 
wheel speeds, will be evaluated. A continuous time LQG compensator is designed and 
the dependence of the compensator gains on the wheel speed is discussed. 

5.2 Lateral System Characteristics 

The dynamic equations of motion of the decoupled lateral system are given in Ap- 
pendix Q, equation Q.6. The yaw angle the io0 angle *, the turntable speed r, 
relative to the frame and the turntable torque Q T , are the variables associated with the 
lateral system dynamics. 

After substitution of the mechanical parameters from appendix M into the dynamic 
equations, we can evaluate the plant eigenvalues. Asshownin the listing of section QJ.l, 
the openloop system consists of two eigenvalues at approximately plus and minus 3.29 
rad/sec in the s-plane. Tie unstable eigenvalue is associated with the lateral inverted 
pendulum mode of the unicyde robot. Two other modes near s = 0 are shown in the 
print-out. One is associated with the yaw rate of the unicyde and the other with the 
turntable angular speed relative to the frame. The modes at s = 0 associated with the 

46 



4.4. CONCLUSION 45 

. MEASURED. LGT. TIME RESP0NSES 



3. 
•2. 





• 








r- 








! 












. .. 








~ 


I 

!.|J 

-1 —I ..... . 


















1 i ^ 
I I 

1 1 












- 






i 

' i 






j 






i 

i 


j 

| i 
1 



0. 2. 4. 6. 8. 10. 12. 14. 15. 18. 2C 

time(s) 

C0MMANDED WHEEL SPEED = 3 RAD/SE( 



0. 

•2. 













! 




: : 


















^ ! 

A^4- ; 


















! 


















i : 

i 


















1 . 

i 



5, 8. 10. 12. 

time(s)- 



16. 18. 



Figure 4.3: Pitch Angle and Wieel Speed Measurements at 3 nd/sec Commanded 
Wheel Speed 



44 



CHAPTER 4. LONGITUDINAL SYSTEM WITH VERTICAL SENSOR 



n ■ 

1 1 

1 1 

5 — { — - 


1 " " 

I 


l 




| 

i 
j 


I 




i i 

• 

! 




— Ti 

9 *« 




\ 1 










5| : 4 


L 


V 








V | 




1 ! ! 

,L_] 1 


i 

i 
i 

1 r\ 


I 

i 
i 


I 


i 


\ 
i 

1 



- 1 



1.5 
i .0 
0.5 

0.0 

• n s 



time(sec) 
COMMANDED WHEEL SPEED 



= 0 RAD/SEC 





1 . 

1 

1 


1 

1 

l 




i 


! i \ 








1 

I" 
1 




j 




1 ■ : 1 

' i '■ ! 
1 ' 

i i 


Li 




A 




A 






A 




VI J 




X 




V 


\ 


r 


V 1 


1 

I 1 

1 i 


\ 


1 

; i 






i • * 



0. 



4. 5. 



8. 10, 12. 

time(sec) 



14. 



18. 20. 



Fipire 4.2: Pitch Angle and Wheel Speed Measurements at Zero Commanded Wheel 



CONCLUSION 



43 



A Vertical 
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Figure 4.1: Mechanical Design of Frame Pitch Angle Sensor 
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the 25 ids sample period. The dominant pole pair therefore has an undamped natural 
frequency of w n - 1J2 rad/sec and a damping ratio of £ = 0.54. for which the closed 
loop bandwidth is approximately 1.5tc„ = 1-8 rad/sec = 0.29 hz. We notice that the 
frequency of the oscillations in Figure 4.2 are approximately 0.3 hz which corresponds 
to the closed loop system's bandwidth. 

The controlled system has good disturbance rejection. An impulse applied to the 
system by means of a fist blow at the turntable height, merely causes the robot to move 
forward a few centimeters, after which it re-establishes balancing on the spot. The stiff- 
ness of the system is illustrated by the feet that a person grabbing hold of the top of 
the robot and shaking it longitudinally, cannot destabilize the control system as long as 
the maximum control torque on the motor and the linear range of the pitch sensor (± 
5 degrees) are not exceeded. 

The performance of the closed loop system when the unicyde robot moves along 
at a constant commanded wheel speed of 3 rzd/sec (0.585 m/s), is illustrated by the 
measurements shown in Figure 4.3. In this test the lobot still stabilized well, although 
the mairimnm pitch angle reached approximately 1 degree from vertical occasionally. 
This greater error can be attributed to the fact that the linoleum tiled floor surface in 
the hallway where the tests were performed, had a noticeable degree of unevenes*. 

4.4 Conclusion 

These experimental results illustrate that the model for the unicyde longitudinal dy- 
namics is accurate and that, given accurate pitch information, a digital control system 
can perform the balancing function with good stiffness and robustness characteristics. 
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CHAPTER 2. DYNAMIC MODEL FOR THE UNICYCLE ROBOT 



unicyclc into the desired direction- He uses bis knowledge (gained by experience) of the 
nonlinear friction characteristics of the wheel cm the ground to apply the correct torque 
profile to end up in the desired direction. Simultaneously, the rider would lean into the 
turn to counteract the effect of the ground reaction force and the inertia of his body, 
which would tend to let the rider fall towards the direction that he had been travelling. 
To emulate just rm* such a control action on a robot obviously would be prohibitively 
complex. The goal is therefore to identify and emulate only the most important control 
actions of a human rider. 

The first and probably the most important simplification is that we assume that we 
can model the human riding on a unicyde with a linear process. Thereby we choose 
to represent the rider and unicyde by a finite number of connected rigid bodies and 
assume that the relative angular position and rate motions are smalL We also assume 
that the control torques and other kinematic variables are continuous functions of time. 
The restriction to a linear system analysis can be changed later to indude studies of 
nonlinear control methods, but it is reasoned that for the initial studies, we should deal 
with a linear system modeL This not only gives insight into the basic issues involved 
in controlling a unicyde, but also gives access to the most powerful mathematical and 
control sytems techniques available at present. The assumption of a linear system is also 
not so unrealistic, because experienced unicyde riders do not execute large amplitude 
motions when traveling in a straight line or while turning slowly. 

We believe that the most bask configuration that will represent the major parts 
and motions of a human on a unicyde consists of three rigid bodies. As shown in the 
schematic diagram of Figure A.1 it consists of a wheel, a frame to present the unicyde 
frame and lower part of the rider's body and a rotary turntable which presents the rider's 
twisting torso and arms. The meet important way in which a human rider maintains 
longitudinal stability 1b uy means of the torques exerted on the pedals of the wheeL The 
forward and backward leaning action of the rider is of lesser importance and was not 
implemented in the robot in order to simplify the m echanical design- 



Chapter 2 



Dynamic Model for the Unicycle 
Robot 

2.1 Dynamics of a human riding a unicycle 

are noakceax. As is the case with mast (kill* teamed hu k 
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2.2 Mechanical design of the unicycle robot 

The unicyde robot that was designed, consists of three parts: a wheel, a frame on which 
the drive motors, sensors and electronics are mounted, and a battery pack on top of the 
frame which can route abont the vertical axis. 

The robot was specifically designed with mass and dimensional properties similar to 
those of a hu*nan riding on a unicyde. This allowed interesting performance compar- 
isons, since the open-loop time constants in the two cases are similar. Although these 
dimensions and weight make the robot less transportable, it gives the desiguc more 
freedom to select equipment without being too concerned about physical size. Enough 
batteries could also be mounted on the robot to supply electrical power for extended 
periods of up to two hoars of continuous testing without recharging. 

Consider a young person of about 50 kg (110 lb) mass. The mass can be roughly 
divided equally between the body above and below the hips. The upper part of the 
body that performs the twisting motions will be approximated by a 25 kg cylinder of 
0.2 m radius. The moment of inertia about the vertical axis is therefore approximately 

W = -Af* 2 == 0.5 kg-.* 3 

This compares well with the turntable (battery pack) inertia of /J = 0.5028 kg - m 2 . 

The rider's leg,, . L are usually slightly bent when resting on the pedals, are 
approximated by two 12.5 kg cylinders of r = 8.5 cm in radius (r) and spaced a distance 
(d) of 0.1 m apart. The moment of inertia about the vertical centerline between them 
is given by 

T Ug . = 2[imr 2 + m#\ s 0.34 kg - m 2 
This inertia is close to the robot's frame plus electronics inertia of if = 0.3635 kg - 

m 2 . 
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robot is shown in Figure 2.1 and more detailed drawing are contained in Appendix L. 
The microprocessor rick is mounted in the middle of the frame between the two motors 
and the two servo amplifiers on either side of it on the outside of the frame. 

2.3 Dynamics of the unicycle robot 

The unicycle robot may be modeled as three interconnected rigid bodies as shown in the 
schematic diagram of Figure A.1. The various coowlinate transformations required to 
describe the relative angular motions of the three rigid bodies are defined in Figures A3, 
and A.3. 

In Appendix A the dynamic equations of motion are derived by using Newton- Euier 
mechanics. Each part of the unicycle is considered as a f .-ee rigid body with gravity 
forces, control torques and reaction forces and torques ten adjoining bodies acting cn 
it. It also includes the dynamics of two sensor pendulums R and S mounted to the 
frame which can freely swing in the longitudinal and lateral directions respectively. The 
derivations make provision for mounting the sensor pendulums anywhere on the unicy- 
cle frame. 

The equations are generalized to include the situation where the center oi mass of 
the frame is displaced away from the vertical centerline of the robot. Inclusion of the 
products of inertia into the dynamic equations of motion would significantly compli- 
cate the mathematics: Therefore the robot was designed with its mass distribution as 
symmetrical as possible about the longitudinal and lateral planes which intersect in 
the vertical centerline. This would cause the products of inertia to be negligibly small 
or zero (section 7-2, (Greenwood)). It is assumed that the wheel rolls on the ground 
without slipping, which places a nonholomk constraint on the dynamic equations. The 
dynamic equations are derived for a nonzero nominal turntable speed, which produces 
coupling between the longitudinal and lateral plant dynamics. 



22. MECHANICAL DESIGN OF THE UNICYCLE ROBOT 

The center of mass for the unk^cle robot above tie wheel «le i. given by 

which would be located at approximate!, * height fo , , ^ ^ ^ ; _ 

The mechanical parameter, of the robot and the measumnent method, are given 
h Appendix M. The calculation, above do not include aD the inertia proper*, of * e 
-icrde plu. rider, and only serve as « mdiatkn that ^ ^ ^ 
« "tempt was nude to let the mechanical .ytem approximate the real life situation. 

The reader may wonder why the heavy battery pack is placed at the top of the 
onicyde n>bot. P,acing the center of ma- high not oniy emuU*. the tea, case of a 
human on a unicycle, but i, a»o advantage™ from a control system, point of view. If 
we confer the unicycle « a simpte inverted pendulum, the higher the center of mass i, 
theurger is the^owntimecou.tant^the^oop^ten, This permit, a slo-er 
-npang frequency m the control microp,oee«or, so that it ha. more time to complete 
the^cuJaUonsofthebalancealgorithn, Furthermore, during balance tecovery action, 
the wheel drive ton,u. ha. to accelerate the wheeJ until if axle is below the center of 

° f * - « 1 * the effective inertia that the torque ha. 

to accelerate depends on the height of the center of ma» of the superstructure (frame 
*d turntable). When the center of mas. of the superstructure i, just above the wheel 

; ^ * » W afl of the superstructure', a. 

wefl a. the wheel', inertia during stabilization action,. If, on the other hand, the snpe, 
structuxeVcenterof mas, i, high above the wheel axle, only the wheel', inertia need, 
to be accelerated (see equation, L.4 and L.5). 

The frame of the robot was made out of aluminum. All of the more complicated 
P»«, were made on a numerically coded milling machine to provide a light bu* -~d 
structure to which the motor., gear system,, servo amplifier,, computer rack, senL 
electron.es and the rotary battery pack could be mounted. A drawing of the unicycle 
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equal to zeic for tie whole unicyde. 

• se( the vertical component of the moment about the vertical centerline equal to 
zoo for the whole unicycle and include the applied ground friction torque. 

. set the moment about the axle of the wheel equal to zero for the frame plus 
turntabie and include the applied torque Qw and wheel drive friction torque. 

. set the moment about the axle of the turntable equal to zero for the turntable and 
include the applied torque Q T and the turntable drive friction torque. 

The resulting equations in sections and C.3 serve as an additional confirmation 
that the dynamic model for the system j derived correctly. 

2.4 Selection of actuators and the drive system 

One of the aims of this research project vas to build a robot that can stabilize itself 
with jut any connections 10 its <«™ other than the wheel touching the ground. 
Therefore both its actuators and power source should be on-board. Direct current mo- 
tors and batteries offer a convenient solution to this requirement. 

The choice between direct drive or geared drive systems was dictated by cost While 
the former offers advantages of a simpler mechanical design and none f f the nonlin- 
earities associated with geared drive systems, these motors were too expensive for the 
budget of the project. The direct current servo motors ( (Infranorj) that were selected 
have low inertia armatures which allows high angular acceleration of the rotor and can 
run at low speeds with foil output torque and no cogging. A 316 watt motor was se- 
lected for the wheel drive system Kecaase the robot is expected to stabilize itself while 
continuously running at a forward speed as high as 3 metes per second (10 km/h). 
A smaller 151 watt motor was selected for the turntable drive system. The maximum 
speeds for these motors are 7000 and 10 000 revolutions per minute far the large and 
small motor respectively. 
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The dynamic equation, fo, ^ three ^ rf ^ ^ twosensor 

dulums are obtained after eliminating ^ the ^ ^ ^ ^ moiMftta ft 
result, : h an deventh order.y.tem with two feeing f^bn., tke whed — ^ ^ 
turntable torque The equation, are .ummarized in action A12 and p^ted* nate 
sr-ace form in Appendix D. 

As a means of checking the equation, of motion, the derivation was repeated by 
us.ng W equation* in Appendix B. This method is based on the physical prin- 
cple that the time rate of change of the scalar generalized momentum * i, e*»l to * 
generalized force <?, due to the applied force, plu. an inertial generalized force dee to 
motion in the other generalized coordinate, (section «. (Greenwood)). Often the Q 
are denvable from a potential function, which is the potential energy of the unicyle in 
the gravity field of earth, in the present case This method avoids the need to elimi- 
nate all the reaction force, a. in Appendix A. The no-slip condition on the wheel is a 



section B ,3.1. 



— ~- » -d=dsd iito L^r^c's equation as shown in 



In the Lagrange's method derivation, we did not include the sensor pendulums and 
****** of the frame since we ^ ^ ^ ^ ^ ^ ^ 

The summary of the dynamic equation, of section B.4 indeed confirm, that the previous 
equations are correct. 

A third very useful method for checking the dynamic equation, of motion is de- 
*nbed in Appendix C. It ir based on D'Alembert's principle which state, that the 
law, of static equilibrium apply to a dynamical system if the inertial forces a, wefl as 
the actual external force,, are considered as applied force, acting on the sy«em (sec- 
fon 1-5, [Greenwood]). After determining the D'AWoert forces, D'Aiembert torques 
and gravitational force, on the three part, of the unicycle, the following procedure 
(IBryson 3] provide, the dynamic equation, of motion: 

• setthetwoh rizontal components of the moment about the ground contact point 
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A precision cable and polyurethane chain drive system and aluminum gears were 
used to reduce the motor speeds and increase the motor torques. This system requires 
uo lubrication and the gear ratios can easily be adjusted by using different belt and 
gear sizes. An effort wa* uiade to select optimum gear ratios for the drive systems. For 
the wheel drive system it is desireable to select .a gear ratio which matches the motor 
rotor inertia to the effective inertia of the longitudinal system dynamics, so that max- 
imum wheel acceleration can be obtained. This will enable the quickest recovery from 
longitudinal balance disturbances. Appendix K shows how the wheel acceleration as a 
function of the gear ratio can be derived from the dynamic equations of motion. For 
the robot parameters listed in Appendix M, a 24:1 gear reduction is near the optimum 
value for *he wheel drive system- 



Lateral system stability is maintained by continuously steering into the direction 
that the unicyde is (ailing. The optimal gear ratio for the turntable drive system would 
therefore be the ratio that gives m^rinmm yaw acceleration. Appendix G uses the lat- 
eral system dynamic equations of motion and plant parameters to calculate that the 
optimal gear ratio would be approximately 72:1. The practical limitations of the three 
stage gear reduction system that was constructed limited the gear ratio to 60:1, but this 
results in a maximum yaw acceleration that is 93% of the optimum. 



Pulse width modulated amplifiers ([GalH Motion]) were used to drive the servo mo- 
tors. The particular models that were used modulate the 48 V d.c supply from the 
batteries to supply a regulated current to the motors. They can continuously supply 
10 A to each motor. The servo amplifier receives a reference voltage from the digital- 
to analog interface card in the on- board microprocessor, which commands a torque in 
the motor. The digital computer interface electronics is described in more detail in 
Appendix L 
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Analyses in section N.3 and Q.3 show that the inverted pendulum modes are observ- 
able from measurements of accekrometers mounted on the unicyde frzme. We decided 
to use this type cf sensor because it would allow the robot to ride on inclined terrain 
and accelerometers of adequate sensitivity could be constructed at an affordable cost. 
Simulations have shown (see later chapters) that the maximnm lateral and longitudinal 
accelerations ^are in the order of 0.1 m/s 2 . Appendix H describes how servo accelerom- 
eters with an accuracy of about 0.0033 m/s 2 were constructed by careful mechanical, 
optical and electronic design. 

If we are prepared to do tests on horizontal surfaces only, the unkyde can pull a 
light carriage with position sensors to measure the roll and pitch angles directly. We 
constructed such a sensor by means of a small magnet and a Hail effect transistor to 
measure the unicyde frame's pitch angle relative to a horizontal floor surface. This 
provided an inexpensive way to demonstrate experimentally that the unicyde robot 
could be stabilized if the vertical orientation information was available to the control 
system. 



Chapter 3 



Longitudinal System with 
Accelerometer Sensor 

3.1 Introduction 

A significant simplification of the control system design can be obtained if the lateral 
and longitudinal system dynamics can be decoupled. Inspection of the system dynamic 
equations presented at the end of Appendix A, shows tK*t it is indeed possible to de- 
couple the lateral and longitudinal dynamics if the nominal turntable angular rotation 
speed qo is zero and the unicyde frame has left-right symmetry about a vertical plane, 
i.e. r x = r 7 = 0. The first requirement implies that the turntable rotation speeds should 
always be so slow that the gyroscopic effect of this rotating inertia does not significantly 
couple lateral motions into the longitudinal dynamics. Previous research efforts pgnchi] 
have in feet used a feat rotating gyro on the unicyde to slow down the time constants 
associated with the felling down of the unicyde. Since the purpose of this thesis is to 
emulate the control method of a human riding a unicyde, where this form of gyroscopic 
stabilization is absent, the requirement that ifo must be zero is compatible with the goal 
of this research. 

The second requirement that the center of mass of the unicyde must be on the line 
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of geometric symmetry has ^ ^ by the meduaiaJ ^ ^ ^ 
robot as discussed in the previous chapter. 

The dynamic ^auons of motion for the deconpW fengitudinal system are given in 
Appendix N, equation. N.3 and N . 4 . The pitch angk *, the wheel perturbation speed 
ft and the wheel drive toreue Q w are the varices associated with the longitudinal 
system dynamics. 

3.2 Longitudinal system characteristics 

After substituting the measured mechanical parameter, into the dynamic equation 
we can determine the longitudinal system characteristic As shown in section NJ the 
ergenvalues of the openloop system consist of two pole, at approximately plus and mi- 
-us 7 «d/s in the .plane the unstable pole is associated with the inverted pendulum 
mode of the unicyde, when it laU. forward or backward. The third eigenvalue „«r 
tbe ongra of the s-plane corresponds to the rigid body horizontal velocity mode for th. 
unicyde a. a whole. The fourth mode associate* with the wheel's angular position is 
not shown because we do not intend to contro. the unicyde'. position. Inspection of the 
-odalcontroOab^matrixshow, ^a-^a*,^^^^ 

A tachometer is mounted on the wheel drive motor shaft. We assnme that drive 
belt dasticty and backlash are negligible so that the tachometer measures the relative 
S p«d between the whed and the unicyde frame. The transfer function from the motor 
torque to the tachometer measnrement has two zeros at approximatdy plus and minus 

23 r3d/S " Phy8kal * ~o location, can be interpreted after a few 

calculations: 

Simplify eauatioss S J and N.4 to 

Ai*+/„n = jo-q w (31) 
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The viscous friction constant f w has been ignored since it has no effect on thereto 
locations. 



Since the tachometer reading is 
we can rewrite equations 3.1 and 3.2 to 



(3.3) 



(Ai + Aj)*+7i2* = JB-Qw (3.4) 
(/12 + +Inift = Qw . (3.5) 
Ta*e the LapUce transform of the above two equations, eliminate 9(s) and solve 

Qw(s) *[(/n/«-/? 2 )^-y/„] 

The zero locations are at 

- j- / ^ (mprp mrrr)g 

V 27 + mwr^ + /* + mfirw + r r ) 2 + IJ + mrfrwr + rr)3 



(3.6) 



(3.7) 



If we close a tachometer feedback loop with proportional feedback and command a 
tachometer speed y c 

Qw = K{y c - y t ) (3. 8 ) 

the root loci of two of the closed loop poles approach the zero locations and the other 
closed loop pole moves to infinity as K becomes large. This implies that with the tight- 
est feedback loop dosed on the tachometer measurement, the time constant associated 
with the instability of the longitudinal system can at best be lengthened to that at the 
zero locations. With a tight feedback loop a command of y c = C implies no relative 
movement between the wheel and the frame. In this locked wheel situation the onicyde 
falls over more slowly (longer time constant) than the free wheel condition (shorter time 
constant associated with the open loop pole positions). Notice that equation 3.7 gives 
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exactly the dgenv^es assorted with a unicyde pitching ove, with a locked wheeL 

An accderometer mounted on the unicyde frame measure, the specific fere at the 
point where it is located. It. output i, a signal given by equation N.16: 

where ^ i. the wheel radiu. and i. the height above the wheel axle where the ao 
celerometer is located. The transfer function from the wheel torque to the accepter 
output has an equal number of poles and zeros (3) due to the di«t ^through term 
from the wheel torque to the accelerometer measurement (see equation N.16). One of 
the zeros is at the origin of the .-plane and ^location, of the other tu* zero. w . 
function of the accelerometer height r*. The location, of the* zero, have a significant 
effect on the dosed loop .y.tem behaviour and we now proceed to determine the* zero 
locations. 

The transfer functions ^ » can be calculated from the longitudinal 

system equations 3.1 and 3.2: 

_ (Ar + W-7 

*l(Ai/23-4).3-// a j (3.10) 

J^L = -(/»+/„). 

*U4i/« (3-11) 
Uphce transform 33 and rabstitute 3.10 and 3.11 into it: 

VW{3) 4(Ai/ E -/^-// a ] (3.12) 

For the ca* with nonzero friction, the pole will not be exactly at s = 0 and thus 
wUl not be canceled by the zero at the origin. 

The zeros of the accaerometer transfer function are given by 

s = 0 ■ 

and 3 = ±j. 9(Ji2 + In) - rw J 

VUu+Az)nr-(/ lJ + / a )r*, < 3 - 13 ) 
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By substitution of the parameters from Appendix M into the equation above we see 
that the pair of zero. « « innnjty for the ,«ekroiEeter monnted ^ ^ ^ ^ 



Uii + ia) 

= 0.6675 m (3.14) 
If an accekrometer we pbced at r ra = ( rja)ao , it would have zero iaftial mponse 
to an impulse on the wheel torque. With the accekrometer »t this height the di- 
rect feedthrough term from the control torque Qw to the accekrometer output ^ be- 
comes zero. It is confirmed by equation N.16 where the coefficient of Q w , namely 
r tcP\ + rwG 3 , vanishes when rjo = (rta)^. 

For r m < the aecelerometer transfer function has a pair of complex zeros 

and for r» > (r^ the zeros are on the positive and negative real axis. The location 
of the zero, is thus very senritive to small deviations in r„ if r ra i, m the vicinity of 

( r ftj)oo- 

Another undesireable situation occur, when r to is such that the aecelerometer zeros 
cancel the inverted pendulum poles of the plant. These modes then become unob- 
servable &om the accekrometer measurement, which defeats the purpose for which the 
aecelerometer was used in the first place We can solve for this critical height from 
equation 3.12: 

= // » r " f +(A 1 /z 3 -f?,k 
//a 

= O- 721 m (3.15) 

The physical explanation for the existence of this point of unobservabffity is that, if 
the unicyefe was allowed to M over in the plane of the wheel, this point would move 
straight down, Le. it would have no horizontal acceleration. If we think of the spring- 
mass analogy of the aecelerometer, it is dear that such an instrument mounted at this 
height, with its sensitive axis pointing along the direction of the unkyck travel, will 
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WaZer ° ontpot - ^erometex mounted at Urn point « the unicycle b aaalagou, 
to the case where an aecelerometer is mounted on tie bob of a simple pendulum. 

A method to identify the longitudinal system dynamic model experimentally b de- 
scribed in .ection N3.2. One of the problem of identifying an unstable plant « tha.it. 
variables oo not stay within small perturbation rang* for very long (in the order of o^e 
second in this ca~). The small angle, -sumption is bask to deriving a valid linearized 
; model for the unicycle dynamic (Appendke. A, B and C). A partial identification of 
the system transfer function, wa. obtained by hanging the unicycle up«de down by it, 
wheel. The dynamic model then change, from an inverted to an ordinary pendulum 
to which we can apply sinusoidal test signal, and measure frequency responses. In sec- 
tion NJ.2 the conclusion is drawn that the actual plant model is acceptably close to the 
theoretical model because their frequency mponn. agree to within 2 dB in gain and 10 
degree, in pha« over the frequency spectrum of signals ?r «en t m ^ dynamic ^ 
daring typical maaneven. 

3.3 Compensator design by successive loop closure 
3.3.1 Nominal design 

Control systems designed by succe«ve loop closure technique, can produce compen- 
sators of low order. This i. advantageous when the compensator i. implemented in a 
microprocessor, which take, a finite time to calculate the control command. The shorter 
this time is, the fester sampling rate can be used, which usually improves the quality of 
the dynamic response Several other consideration, for the sampling rate selection are 
discussed in section 0.1. 

The longitudinal dynamics are modified to include the delay time between the in- 
stant that the measurements are made and the time that the control command is issued 
(see section NJ.2). 



B.2. FRAME 



99 



The transitional kinetic energy of the wheel is: 
1 w - ^wto • r 0 

2. Rotational kinetic energy of the wheel: 
absolute angular velocity of the wheel is: 

= 4>v>\ + (w + i*4>)i>2 + ttote* (B.9) 
The rotational kinetic energy of the wheel is: 

iy = ijf^ + + + * ^ (B . 10 ) 

3. Potential energy of the wheel: 

= mwgrwc<t> (B.ll) 

B.2 Frame 

1. Translational kinetic energy of the frame: 

The position vector from the origin of the inertial reference frame to the 
center of mass of the fra^e is: 

f F = r 0 + r F f z 

= [x + (nv + rf c0 js<£stf + rjrs0c0]ai 

+ fa - (nr + rpc0)s4>ci> + rjrs&tffo 

+ ((ny + rf^^laa (B.12) 

= ±f«i + yr«2 + ir«3 (B.13) 
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APPENDIX B. EOMS BY USING LAGRANGE? METHOD 
where 

t rrffcfc* - + _ ^ + 

(B.14) 
(B-15) 

The translation*! kinetic energy of the frame is 

^ = 1^ + ^ + 4) ( B. 17) 
2. Rotations kinetic energy of the frame: 
♦ic absolute ang^ velocity of the frame 

A W F = 003 + ^6,+^/, 

(B.18) 

The rotational kinetic energy of the frame is: 

3. Potential energy of the frame: 

Vp = mwgzf 

B.3 Turntable 

L Tranalational Idnetic energy of the tnrniable: 



(2.19) 



B.J. TURNTABLE 
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The translation^ kinetic energy of the turntable can be found by simply sub- 
stituting tr for r F in equations B.13 through B.17: 

*r = x + Tw(fc4ml> + ^c^) 

(B.21) 

yr = y + rw(i^1> - 4*4x1>) 

+ r T [9($^ + c^s^) + + ^ ) - Wc^c^] 

(B.22) 

zr = -rnr^ - rr{^c^ + <Ms^) (B.23) 

The translation*! kinetic energy of the turntable is 

I? = + # + (B.24) 

2. Rotational kinetic energy of the turntable: 
the absolute angular velocity of the turntable is: 

= (**-**«)/,+(« + W)/ 2 + (^ + ^C^ + i>)/3 

(B.25) 

The rotational kinetic energy of the turntable is: 



(B.26) 



3. Potential energy of the turntable: 

The potential kinetic energy of the turntable can be found by substituting 
rj for r F in equation B.20: 

V F = m T g(rw + rrcBty (B.27) 
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B.3.1 The LagrtEgsan and Lagrange's equation 



The Lagrangjan is 



(B.28) 

and Lagrange', equation for a system with » genemhzed coordinate., «, and m 
nooholonomic constraint equations, is: 



±fdl\ dl A 



(for t = l,2 it) 

where Qi are generalized force, which are not derivable from a potentionai 
function (the applied torq ne , from the moUm and friction in this case) 

where are the constraint equations of the system. 

See [Greenwood] chapter 6. [Par,] chapters 6 through 8, [Rosenberg] chapter 15 
and [Wbittaker] chapter 2 for details. 

Note: 

It would be tempting to substitute i and y from the constraint equation. B.2 into 
the kinetic energy term,, m order to eliminate two of the generalized coordinates 
(x and y) and simplify the system of equations. 

As pointed out in [Wnberg], chapterU, this will lead to the wrong answer since 
the constraint equation, are nonholonomic (not iategrable). Embedding o; con- 
stant equations are only possible with holonomic constraint equation,. Trying to 
mb€d nOnh0l0n0mic ^^ns to eliminate the use of Lagrange m=l- 

taplier, violate, the dynamical principles on which the derivation of Lagrange's 
equate are based (the principle of work done during virtual displacements) 
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Simplify the expressions for the total translational kinetic energy by defining: 

A x = i+rw(*c^ + ^c^) (B.31) 

+ Mri*4>ctt> - s0s1>) + (B.32) 

Ay = y + rwr( 0s^s^ + oc^c^) (B.33) 

+ ^ + cfc*iW-W^ (BJ4) 

A, = rwfrufi (B.35) 

= -(^ + ^ (B.36) 

The total translational kinetic energy then becomes: 

= f m r + m T )(A| + ^ + Aj) 

+' (™FTF+mTT T )(A x B x + A v B v + A Z B X ) 

+ j(mjrfJ. + mr4X*2 + ^ + «J) (B-37) 

The total rotational kinetic energy is: 

+ \tf + i?)(W-*<&) 2 +\(i(+%){8 + i™i>) 2 



The negative of the total potential energy is 
- Vtot = — (row + (Rf + mx)nvsc^ - 
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define: 
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The Lagrangian then becomes: 



1 = TZ t + T£-V„ 

= + Al + + mi(i4r ^ + AvB> +Ag) 



Hnder stabilued conditions, the an£i« « M w ^ 

The small angle > Wmim ^ ™ ^ 

^ aPProamaticns in* a * cos* * i a* * „ . 
^pUfy the equations. * ^ ~ ^* * 0 etc. are used to 

i- qi = x: 
dl 

(B.44) 
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31 (S) ? mu ^ i + ^**) + m i(' c *+£*0 (3.45) 
where the assumption < 1 has been used to simplify the equation. 



from equations B 2 and B.4 



3L 

o7 = 0 (BM > 



a n = 1 (B.47) 
*i2 = 0 (fi.48) 



substitute into Lagrange's equation: 

m tot (z + rwiei,) + m t (^ + is*) = A a (B.49) 

2. q 2 = y: 

— = m to «(A v ) + m\B 9 

(B.50) 

d /3L\ - - 

dtVdjJ S m Uv - r^<*ctf ) + m x {esxl> - 0ctf) (B.51) 
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^ = 0 (B.52) 



an = 0 (B.53) 

*22 = 1 (B.54) 

substitute into Lagrange's equation: 

mtotiv - rwfctj;) + mx(^ - £ctf) = A 2 (B.55) 
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3. q3 — an 



dt (H) ~ ^(w+^ + ^J 



substitute into Lagrange's equation: 

if® = -rwcrb\ x - rwmfrA, + Q v 



(B-57) 



(B.S8) 



(B-59) 
(B.60) 
(B-Sl) 



(B.62* 



. + + (if + Jf )(* - <*) + (Jf 4 /J)*, + (B 63) 



. 
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a u = 0 (B.66) 

a 74 s 0 (B.67) 

Q 4 = 0 (B.68) 

substitute into Lagrange's equation: 

(m^trw + nn )(£stf - yctf ) + Jj° f + m tot r£r + 2x71^ + ro 2 )^ + /Jitf 

= /^^^(mtotr^ + m!)^ (B.69) 

5. q 5 =s : 

^^(mtotrw+mO^ic^Tys^+if *V + /f'tf + /J* (B.70) 



dt [d*) 1 



(nitotrw + mj ){#xctf + ystf) + <#xctf - ystf]} 



§f S (mtotnr + mxMictf + y^) + <K*z* + ys^)l (B.72) 

<7V 



a 15 = 0 (B.73) 

ajs = 0 (BJ4) 

Qs = 0 (B.75) 

Notice that Q$ ^ -Q T because the torque Qr does not act around the axis 
about which the coordinate under consideration (t£) rotates. 
Substitute into Lagrange's equation: 

I%*1> + /f * + 1?*$ - (m^rw + mi )#xcy + ystf ) =r 0 (B.76) 
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«• q« = 8: 

dl 



±(dl\ 



«i« = 0 
«w = 0 

Sab8titnte » to ^ge'. equation: 

<l7 = If. 

d (dl 



5t [5*)* 



at 

&7 = ° 



°J7 = 0 
<*77 = 0 

<?r = <? T 



(B-77) 

(B.78) 
(B.79) 

(B.80) 
(B.81) 
(B.82) 

(B.83) 

(B.S-1) 
(B.85) 
(B.86) 

(B.87) 
(B.88) 
(B-*9) 



B.J. TURNTABLE m 

Substitute into Lagrange's equation: 

jJ(tf+*) = q T (B.90) 

Eliminate x and y coordinates from the dynamic equations: 
from B.2: 

x = rurtictf (B.91) 

y = rwias^b (B.92) 

x = rw(wci> - wrinrj?) (B.93) 

V - rwiwsj + ibi>at>) (B.94) 

Eliminate x from B.49: 

mtotrw(&c$ - -f tetf ) + m^fe^ + 4*$) = A x (B.95) 
Eliminate y from B.55: 

m^r^^stf + T^ctf - ici>) + m^fotf - icrf,) = \ 2 (B.96) 
Eliminate X x and A 2 from B.62: 

(i!? + mtotrly)* + (rnpTf + mrrj^B = (B.97) 
Eliminate x, y, x and y from B.69: 

(B.98) 

Eliminate x and y from B.76: 
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bat from B JO: 



Eliminate £ and y from B^3: 
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B.4 Si 



nummary of unicycle dynamic equations 

Note that: 

* = flo + n 

*• = Vo + r) 



(B.101) 
(B.102) 



B.4.1 Lateral system dynamic equations 
Wheel and Frame Yaw Dynamic: 

+ (B . lfl3) 
Gyroscopic Coupling Dynamics: 

V? + If + If + (m w + mr + mT y w + 2rw{mFrF + mrrT) 

= ^ + ^ + ™F + m T y w + {mF r F + mrrrJnejfto^ 
+ l r ( m ^ + m F + mT)rMr + ( ^ + mrrr) ^ 

(B.104) 

Turntable Rotational Dynamica: 

/f(*+*) = <? r (B105) 



BA. SUMMARY OF UNICYCLE DYNAMIC EQUATIONS 
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B.4.2 Longitudinal system dynamic equations 
Frame Pitch Dynamics: 

[/f + /J 4- m^rj. + mjr\}fi + (mpr? + mrrx)nrft 

(B.106) 

Wheel Rotational Dynamics: 

[I? + (mw + + m r )r^]n + (m^p + mrrr)rwO = Giv (B.107) 

These are the same dynamic equations as those obtained in Appendix A where 
Newtonian mechanics were used to derive the equation of motion. 
The cross coupling terms are absent in the results of the Lagrangian derivation 
because they were not taken into account. 



Appendix C 



EOMs by using D'Alembert's 
Principle 



C.l Procedure 

- Determine the following tatvm ^ ^ 

(1) D'AIembert torques acting on wheel, frame and tnrntable. 

(2) n'AJemhert forces acting on wheel, frame and turntable. 

(3) Gravitational fortes acting on wheel, frame and tnrntable. 

~ Determine five equations of motion: 
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(1) - (2) Set the two quasi-horizontal components of moment about P (wheel 
contact point with ground) equal to zero for whole unicyde. 

(3) Set the vertical components of moment about P (wheel contact point with 
ground) equal to zero for whole unicyde, and indude the external friction 
torque - /c^. 

(4) Set moment about axle of whed equal to zero for frame plus turntable 
and indude applied and triction torques -Qw - fw(& - B). 

(5) Set moment about axle of turntable to zero for turntable, and indude 
applied and friction torques Qr — /r*7« 

D'Alembert Torque: 

- Wheel: - 

- (/T* - I?no1>)*i - l?ilw2 - (I?j> + iTSUj)** (C.l) 

- Frame: 

-i?*fi-i(ef2-f[*h (C.2) 

- Turntable: 

- {If* + lW)h - (/?» - - + i))h [C2) 

D'Alembert Forces: 

- Wheel: 

-mwrw[Sl*>\ +(fto^-*)tP2] (C.4) 

- Frame: 

- mfirwtl + r^)/! - mrRrw + r F )<£ + nirftotfl/z (CJS) 

- Turntable: 

- mjirwil + TT6)f x - mttrwflo^ - (rw + r T )<*l/i (C.6) 
Gravitational Forces: 
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- Wheel 

-■Mr*-**.-**) (C7) 

- Frame 

- Turntable- 

6, Component of Moment about P for Whole Cycle 

~ (if* + /f + mrfr* + rr)^ - + ^ + 

(CIO) 

«2* Component of Moment about p fo r wv^ Cvde 
3 = -i^fl - m w r 2 w il 

*3 Component of Moment abont P for Whole Cycle 

■ = -(/f * + jTiW) - /f * - tftf + « _ ^ (C r2) 

A Component of Moment abont 0 for frame and Turntable 
0 = -l[9 + mrrfigt - rwSl - TF 0] 

(C.13) 

A Component of Moment abont Q for Turntable 

0 =-#(*+*)+<?r-M (CJ4) 



C.2. LATERAL EQUATIONS OF MOTION us 

Subtract equation C.14 from C.12: 

* = -{I?* + I?SU6)-I$j> + jT*l-QT-fGi> (C.15) 
Subtract equation C.13 from C.ll: 

0 = -[/f + (m«r + mf + mf )rHr]n - (mff f - mrfxjfuri 

- fw{$l-9) + Qw (C16) 

C.2 Lateral equations of motion 

from C.15: 

Vi + tf)* = - /c* + /r*7 - Qt (C.17) 

from CIO: 

(jf + if + /f + mwrrJr + m^p^ + r F )- + mr{rw + 'r) 2 ]£ 
= [i^ + mwrJr + mfTwr(rHr + r F ) + mrrw(rw + T)]fto0 
+ [m*nv + mf^tv + r F ) + mr(rw + r r )]$<£ - ilmfi (C.18) 

from C.14: 

tf(* + i?)==-/r/ + CT (C.19) 

C.3 Longitudinal equations of motion 

from C.13: 

= /JW+(mirrir + mTrT)^ + Ma-^)--(?^ (C.20) 

from C.16: 



APPENDIX C. BOW BY USING D'ALEMBESTS Panaris 

m»g nonmu d«« feed i£» „. . , ~ «M ram. 

»«o. *a~d . App,di, A and B. In App^ A Nevtoo'g * 

and Lagrange's eqwtioawa, wed. ^ P 
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State Space Form of the EOMs 



The system dynamic equations from Appendix A can be written in matrix form 
as: 

/x = f* + Gu + Kw (D.l) 
The matrix equation can be converted to the standard state space form: 

x =■ r l Fx + r l Gu+r l Rw 

= Fx + Gu + Kw (D.2) 
The dynamic equations in matrix form are shown on the next page: 
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APPENDIX D 



STATE SPACE FORM OF THE EOMS 
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-1 
0 
1 
0 
0 
0 
0 
0 
0 
0 
0 



0 
0 
0 
0 
0 
0 

-1 
1 

0 
0 
0 



Qw 



0 

0 
0 
0 
0 

0 
0 
0 
0 



h 
o 

-St 
0 
0 
0 
0 
0 
0 
0 
0 



where the coefficients of the matrices are : 
hi = -[mrirw + r F )ri] 

ha = Itf+if+mti'i + Ti)} 

hs - -mprwri 

/ij = -I7O0 



0 

.0 
0 
0 
0 
0 

fw 
-fw 

0 

0 

0 



9 
•to 
fto 



hi = +I\ +/f + (mnr + inF + n»r)*fr + n >TTT(2ny+tT) 

+ mfTjr<2rwr + rjr) + mprj] 

h.i = mfTifj 



'Vp'in 



J 7J =r -mpTiTj 

ha = -mprfTj 

=. {/f +/f + m f Tj. + m T 4 + m r rf] 

J7.8 = (™F*"f + mrrr)r^ 

frao = (mpr^ -f mrrr)$ 

/g^ = —mprwri 

4,7 = (mrrT+ mfT F )rw 

/s3 = [/T + (mw + m F + m T )r^r] 

ft f 7 = [i? + n^ r p( r p- r ro)] 

h* = + 

F*A1 = -[*p + *W*] 



Appendix E 

Attitude by Means of a Sensor 
Pendulum 

^eofasmafl^^ , * - tie deflection 

S€nsor pendulum mounted on tli* 

• t ■ ttVCrtcd P^dulum. Tie mod*! 



E.1 Dynamic equations of motion 

1. Inverted peodnlma dyjuuaics: 
Refer to F^uie E.1: 

D«cription of parameters: 
m r = mass of inverted pendulum. 

= moment of inertia of bn»M« . . 
thecenterofn^ f ^^^--^^ 
' * ° f ^ ~ «" F- 4 *-. «d Parallel to the i, unit 
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vector. 

nip = mass of sensor pendulum. 

J 2 = moment of inertia of sensor pendulum about an axis passing through 
the center of mass, P* f of the sensor pendulum, and parallel to the %2 unit 
vector. 

r[ - distance from the inverted pendulum support point to it's center of 
mass, I*. 

T H - distance from the inverted pendulum support point to the sensor pen- 
dulum's hinge point. 

r p = distance from the sensor pendulum's hinge point to it's center of mass, 

r. 

Acceleration of cm. P of inverted pendulum :>/Sit 
moment balance about 0: 

where the last term in the brackets above is the D'Alembert force. 
Equation of Motion of Inverted Pendulum: 

(Jj + mrrty^mrnge (E.l) 



Laplace transform: 



so that the time response is an unstable exponentially growing function: 

0(O = *(o)cosh»/J + ^sinhw/t (E.3) 
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APPENDIX* ATTITUDE BY MEANS OF A SENSOR 



PENDULUM 



2. Senior pendulum dynamics: 

Refer to Flgce E.2: 
ahgular velocity of p hme 



acceleration of hinge point H: 



(E-4) 



S »-»*Pi 



federation of />• of the ,en«>r pendulum 



- r ^+(* + 7)p ) x(-r p p s ) + 0 



(E-5) 



(E.6) 



moment balance about hinge point H: 
Equation of Motion of Sensor Pendulum; 



7) (E.7) 



define: 



E -~ m p r pS 
rewrite E.7 as : 



« + D7 = E(* + 7 ) 



E.l. 



DYNAMIC EQUATIONS OF MOTION 
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Transfer function from inverted pendulum angle to angle measured by sensor pen- 
dulum: 

Laplace transform for equation C.7 

A(s) _ ( C*-E \ 

Notes: . . 
1. 

lim£=-l 



=> 7|t=oo = -*\t 



so the measured angle is the negative of the attitude angle in steady state, 
*s it should be. 

2. For this case with no damping on the pendulum, there are two poies at : 



s = ±\l — 



I 



where: J ? = J? 
3. The 2eros for the transfer function are at : 



t 
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ma * ^^^^^ 



W ff ^ + «Vr f (r p - Pw)>0;Lt when 
=> 2 complex zeros at 

Sm Figure E J. 
(b) If + nk,r,(r„ - rjl ) < 0 ; i.e. when r ff > r + 

=> 2 real zero at 



See Figure £.4. 

4. It is evident from Firnre r 4. .k 

- critW k * k , MMOr P€adnlum conJd ^ mounted at 

<* cnticaa height, (m) . *l «*wmiea at 

wxur. several methods for calculating tk* m»^;»- # 
itywa now be presented. ^^^on foronobservabfi- 

E.2 Observability matrix 



E.2. OBSERVABILITY MATRIX 
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define: 



P = 



mirig_ 
H + mirj 



_ 9 [_ - miTi[J? + "Wp - r„)) \ 



Q = - 



The state space representation of (he system is: 



(E.10) 



§ 




0 


0 


p 


0 




0 


7 




0 


0 


E 


Q 




7 






1 


0 


0 


0 




$ 


m 7 . 




0 


1 


0 


0 




m 7 . 



on 



x = Fz 



(E.11) 



if 7 only is measured => output matrix: 
M -[000 1) 



Observability matrix is 



O = 



M 






MF 






MF 2 






MF 3 






0 0 


0 


1 


0 1 


0 


0 


0 0 


H 


Q 


H Q 


0 


0 



(E.12) 
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W ™ t " : AttTTVDE BY MEANS OF A SEKSO& 



PENDULUM 



det O = - 



!•. 


1 


0 




i 

0 


0 


0 


0 


0 


B 


-Q 


0 


1 


0 


B 


<? 


0 






Q 


0 


B 7 















system become unobeerv»Me who: 

det 0 = 0= -B 7 

i.e. when: 



(E.13) 



B = 0 



of the mverted pendulum mode, occtu,, is: ^ 



(E.14) 



= // + m/r? - Jf+mrf 



(E.15) 



E.3 Physica, expboatio, for modal unobservability 

W+^fl»-«,. Hpf + ^ ^ 



E.4. UNOBSERVABIIJTY BY POLE-ZERO CANCELLATION 
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define: 

Tennl: [/f + *v>(r p - r*)]* 
Term 2: m p r p g6 

The terms on the right hand side can be considered to be the forcing functions 
on the motion of the sensor pendulum. If the right hand side becomes zero, the 
behavior of B(t) does not influence the motion of -tft), and this is when the unstable 
modes associated with $(t) cannot be sensed by the measurement of 7(1). 
Term 1 is dependent on the location of the sensor pendulum hinge point, and is the 
moment on the sensor pendulum due to the acceleration of the inverted pendukm 
L 

Term 2 is the moment on the sensor pendulum due ot the effect of gravity vector 
change as 6 changes. 

From the dynamic equations of motion of the inverted pendulum I, r;e have: 

- _ m t Tjg 

Substitute in equation above and set right hand side = 0: 

[Jf + m p r„(r p - r B )] j*™ 9 9 + = 0 

( J 2 + m r r j) 

solve for the critical rjy where unobservability occurs: 



[Trier* = + 3 ^ P (E.18) 



which is the same result as obtained in equation E.2. 

E.4 Unobservability by pole-zero cancellation 

The modal unobservability condition can be predicted by calculating the where 
the zeros of ^ cancel the poles of the inverted pendulum: from equation E.9 the 



m. APPENDDCE. ATTITUDE BY MEANS OF A SENSOi PENDULUM 



zeros of ^ are where: 

■r 2 ^ - - - ™*r 9 g 

G ^ + m p r p (r„-r H ) (E.19) 

the poI« of the inverted pendulno are at : 

~ J? + m [r j (E^C)- 

pole-zero cancellation occurs when ah*™. f~ - 

m wnen ^ eo.uat.ons are set eqnaL Solve for 



"V7 rn^rp (E-21) 



E.5 Unobservability by inspection of the P matrix 

The Point of nncbservabOity can be found by inspection of the dvnan.cs .natrix 
« the state space representation of the inverted and sensor pendulum: Fro* the 
F matrix in equation E.ll 

- > r ><> ' > 

/ J+"Vi (E.22) 

*- if ft. of tf above becomes zero, the d^nic of , k « „ 

fc- b^e^of.,.,^,^ ^oW^o,,,,,^^ 
associated with * by sensing 7 only. 

Solve for the value of r^where the coefficient of 6 becomes = 0. 
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Conclusion: 

Several approaches to solve for the critical location of the sensor pendulum in this 
simple two body system, were presented. When the critical location for a sensor 
pendulum in more complex dynamic systems is sought, it will be evident that 
some approaches may provide the critical sensor position easier than others. 



BY MEANS 




OF A SENSOR PENDULUM 

a -\ a 

Z> YD ttA — H a B 

R lPl + R2P2 + R3P3) 
ignore this reactio- 
force because the 
sensor pendulum 
is nnch smaller than 
the Inverted pendttlro 



.Vl + R2 5 2 + »3i 3 



Fignre E.1: We, P ^ " "x* 




H - H • 



V a 3 



2 + R 3 P 3 








Pi 


cos 7 


0 


-sin 7 


A 


0 


1 


U 




sinY 


0 


cosy 




A 







-■p9'fe+Y)p r «pgp 



!: Senior Pendulum Schematic Diagram 
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2ero location x - 
for r„«ji + uj 

locus of 
zero for 
♦increasing r t 

zero location*-' 





Figure E.3: Locus of Imaginary Axis Zeros ; 



i 4 Function of Sensor Pendulum Height 



locus 
of zero as 

pendulum 
is mounted 

higher up 



f ° r -H- 
zero cancels 
inverted 
pendulum pole 

for r H =r F -t-j p 

ir r 
P P 



Im 




Figure E.4: Loco, of Real A»s Zerc * » Function of W Pendolum 



Height 



Appendix F 

Unicycle Robot Steered by 
Sideways Leaning 

F ! Dynamic Equations 

^d. ^ ^ ^ ^ ^ 
equations of motion. ^* 



- Determine the following ton,n« and forces: 

(1) D'Alemhert torcnes acting on wheel, legs and body. 

(2) D'Alembert force, acting on wheel, leg, and body. 
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(3) Gravitational forces acting on wheel, legs and body. 

- Determine five equations of motion: 

(1) - (2) Set the two horizontal components of the moment about P (wheel 
contact point with ground) equal to zero for the whole unicyde. 

(3) Set the vertical component of the moment about r equal to zero tor the 
whole unicyde, and indude the external friction torque - fci>. 

(4) Set the moment about the axle of the wheel equal to zero for the legs and 
body and indude the applied torque Qw- 

(5) Set the moment about the sideways lean axle of the body equal to zero 
and indude the applied lean torque Qb- 

D'Alembert Torque: 

- Wheel- 

- (JT* - if Qo*)*l - iY&in - (if* + 1% (F.l) 

- Legs: 

-Wj./^/a-^A (F.2) 

- Body: 

- A B (* + ft* - r**h - if*h (F.3) 

D'Alcxnbert Forces: 
-Wheel: 

- mwrwltlvn + (%* - *)*2] (F.4) 

- Le^: 

- m L {rwSl + r L $)f x - m L [-{rw + r L )4> + rwOo^Ui (F.5) 

- Body: 

-m B [f>JU(rtf+r a )*]a-mB[-(ri^^ 



» APPENDIX F. ^CVBOBOTST^tr^n^ 

Gravitational Forees: 

- WhecL- 

- L«gs: 
-Body: 

4 Component of Moment about P for Whole Unicyele 

(F.10) 

* C ° mPOne0t ° f M o»«t *bout P fo r Whole Unicycle 

- tf» + m £ (rHr + _ ^ + 

- / ^+^(nv + r lr + PB)M . rHr6 _ (r/r + rBy) (pn) 
*» Component of Moment about P fo r Whole Unicyele 

o= -C^+jfiW)-/^-/^.^ (FJ2 . 

h Component abont O for Leg, „ d Body 

- / ^ + ^+'B}M-n,n- ( ^ +r ^ ] _ <? „ (F13) 

5j Component about H for Body 

+ «* + ™-«° (F . 14) 
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F.2 Lateral equations of motion 

from F.12: 

(I? + + = -/f fto* - fa* (F.15) 

from F.10: 

[I? + mvtrr^ + jf + m L (rwr + r £ ) 2 ;+ if + m B (rw + r H + r B ) J ]<£ 
+ (A B + m B r B(*W + r ff + r B )]0 

= [I? + m^rnr + mtSw{rw + re) + mBnv(rw + 177 + r B )]fto# 
+ [m w rw ■+ m^rw + a) + ma(ny + rjy + r B )]$# + rn B r B g0 

(F.16) 

from F.M: 

[Jf + m B ra('nir + + r B )]^ + [jf + m B r 2 B ]p 
= m0rHrr fl no^ + mBrB^ + ^)-(3B (F.17) 

F.3 Longitudinal equations of motion 

from F.ll: 

[if + m^r^ + r L ) + J* + m B (r B + r B )(rwr + r H + r B )]0 
+ [iY + mnrr^ + m£,rw(rH' + r^,) + man^r^r + -f r B )]ft 
= [^t^L + m B (r ff + r B )]$0 (F.18) 

from F.13: 



1*2 + + /f + m B (r H + r B ) 2 ]0 + [m L r L + m B (r H + r B )]rwil 

= [™Lri + m B (r H + r B )]g$-Q w (F.19) 



» vranr. owrcvciB robot steered fly szdchmks leaning 
P.4 Lateral system diaracteristics 

le »»i»8 W of tt. rido', bod, (B). 

>b« /, „*. tj!mti , UmUtclmitji 

so that 



I2lf 

m F 4r r (F.21) 



With the hinge point R » distance r„ iw-» »k 

oistance r a from the bottom of the frame where it 
connects to the wheel axle the m«« «f tk t 

«« axie, tne mass of the lower part of the frame is 



The center of mass is at 



The inertia of the lower part of the frame is 



mi= £r* (F.22) 



2 (F.23) 



Likewise, the upper part of the frame has 



mass properties: 



my = 



_ 2r F - rg 



2r^ m ' (F.25) 
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2r F -r H 



irur 



12 



w 2 + 4rl) 



(F.26) 
(F.27) 



Now we can combine the mass properties of the upper part of the frame with that 
of the turntable to find the mass properties of the leaning part of the body (B): 



ma 

If 



my -f my 
rrtu + 

I? + mu(rB -ru) 7 + if + mtirr - r H - r B ) 2 



(F-28) 
(F.29) 
(FJO) 



By calculating the inertia properties in this manner the eigenvalues associated 
with the sideways falling of the leaning configuration unicyde as a whole, will be 
the same as the corresponding eigenvalues of the twisting configuration unicyde 



The lateral system dynamic equations of motion F.15, F.16 and F.17 can be written 
in state space form if we define the state vector 



(F.31) 
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0 


A3 
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0 
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Jl2 
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4> 




-I 




Ai 
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J» 
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0 








0 
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Ju 


Ju 








0 




0 


/» 


0 


0 


0 










-fc 


0 


p 


0 




0 


+ 


0 


Qb 


0 


0 


0 


1 


0 








1 


0 


0 


0 


0 








0 




0 


0 


0 


0 


1 








0 


0 


1 


0 


0 








0 




























(F- 


32) 



where 

At 

A3 

Ai 



if + mBTBirw + tr + t b ) 
lf+ m B r% 



(F.33) 
(F-34) 
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= Ai (F.36) 

I n = /Tx/ 3 c + / 3 a (pj 7 ) 

J12 = m^r^rwHo (F.38) 

= Jis = mB r B9 (F.39) 
V» = + mwrlv + m t tv(fHr + t t ) + m fl rw(ny + r H + ra)}*^ (F.40) 

Ju = [wwrriv +m£,(ny + r t ) + ma(nr + r ff + r B )]^ (F.41) 

<^ = (F.42) 

- -/ffto (F.43) 

J 32 = -h (F.44) 

Multiplication of equation F.32 by the inverse of the first matrix cbntsmsg the 
inertia terms, yields the standard state space form of the lateral system dynamic 
equations; 

x = Fx + Gti (F.45) 

An analysis of the lateral syBtem characteristics is shown in the listing of sec- 
tion F.4.1. There is one eigenvalue near s = 0 which is the yaw rate mode of 
the robot. The other eigenvalue at j = 0 for the yaw angular position mode is 
not shown. Two pairs of eigenvalues reside on the real axis on either side of the 
imaginary axis. The eigenvalues at s = ± 3.29 rad/sec are for the sideways un- 
stable pendulum modes of the unicyck as a whole. These modes are at the same 
frequency as those in the twisting configuration ( section Q.3.1), because we have 
chosen to use the same robot parameters of Appendix M. Two other inverted 
pendulum modes are at s = ± 9.89 rad/sec due to the leaning part of the bcdy. 
The location of these eigenvalues change as a function of the hinge height, tr. 

The modal controllability matrix CTR in the print-out shows that the yaw rate 
mode is uncontrollable from the lean motor torque. Physically it means that if 
yaw angular momentum is present in the system, it cannot be controlled with the 
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lean actuator. Fortunately this yaw mode is stable due to the friction losses at 
the ground contact point of the wheeL 

The CTR matrix also indicates limited controllability of the main inverted pendu- 
lum modes ( at s = ± 3.29 rad/sec) from the lean motor torque. It is possible to 
stabilize the unicyck by leaning the body, but accurate control ot the yaw motions 
(i.e. steering) cannot be achieved by this control method. 

F.4.1 Lateral system analysis 

0CTC12/LEAKHAR. CTR 

LATERAL STSTEH COTTTOLLED BT LEAH1C 



LATERAL STATES : PHI. DOT; PSI.DOT; BETA. DOT; PHI; BETA 

corraoL nm : leai mno% tobqce cqb) 

OUTS : IETEBS, RADIUS. SECCIDS 



LEAI HUGE HEIGHT ABOVE WHEEL ATT* ( METERS ) : 
KB 

0.70O0 

SGKXIAL WHEEL SPEED (RAD/SEC) : 
OHEGAO 



F. UNICYCLE ROBOT STEERED BY SIDEWAYS LEANEi 



3.0 

OPOLOOP STSTZH WTBICES: 
FUT 



0.0000 


0.7783 


0.0000 


12.9442 


-7.M66 


-0.1259 


-0.0277 


o.oooo 


0.0000 


0.0000 


0.0000 


-1-3688 


0.0000 


-22.33S7 


9S.8731 


1.0000 


0.0000 


o.oooo 


0.0000 


0.0000 


0.0000 


0.0000 


1.0000 


0.0000 


0.0000 



CUT 



0.3308 
O.OOOO 
-3.7467 

o.oooo 
o.oooo 

EICTAL 



-0.0279 
-3.293S 

3.2937 
-9.8930 

9.8930 



EICTEC 



0.0017 
1.0000 
-0.0000 
-S.3600 



1.0000 
0.0386 
0.2606 
-0.3036 



1.0000 
-0.0379 
0.2607 
0.3036 



-0.0902 
-0.0012 
1.0000 
0.0091 



-0.0902 
0.0011 
1.0000 

-0.0091 
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0.0003 -0.0791 0.0792 -0.1011 0.1011 

cm 



-0.0000 
-0.0034 
-0.0034 
-1.872S 
-1.8725 




Rgrne P J: Steering by Leaning 



Appendix G 

Gear Ratio for Maximum Yaw 
Acceleration 

G.l Motivation for the optimal gear ratio 



The turntable is used as a null momentum reaction wheel to steer the body of 
the unicyde- A direct current motor mounted on the unicyde frame drives the 
t^mtable :>.— -H* » <*r*r train. 

The unicyde control system will compensate for lateral disturbances by steering 
the vehide towards the direction that it is felling over. T? wou ld therefore be 
advantageous to calculate the optimal gear ratio of the turntable drive train, that 
would give maximum yaw acceleration of the frame of the unicyde. The relevant 
parameters are shown in Figure G.l 
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146 APPENDIX* OEARBATlOTORMAJOMmYAW^CELmAriON 

G.2 Yaw dynamics with a geared drive system 

Obtaining the dynamk equaW ^ I^r^., mcth ^ : " 
Ut the generalized coordinates of the lateral system be: 

?i = v = yaw angle of the frame 
ft =S> = angle of motor rotor relative to the frame 
to =V = angle of turntable relative to the frame 
the derivative of the generalized coordinates are 

ft = w 

The total kinetic energy of the system is: 

= 2 {/F « ? + ^ + ft) 3 + ^r(«i + to) I J (Gl) 
The constraint equation of the system is 



nr > or * = (G.2) 
where n is the gear ratio of the turntable drive system. 

The nonholonomic constraint equation is augmented to the Lagrange, eunation 
to give 

±f9T\ dT , 

dt\d4iJ'^ St ^ i + Qi (Gj) 
where Q { is a generalized force. 

_ d<f> 

~ % (G-4) 



where * is the constraint equation of the 



system 



YAW DYNAMICS WITH A GEARED DRIVE SYSTEM 
rewritten in differential form: 
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We can readily identify the a, coefficients: 

a\ = 0 
a 2 = 1 

a 3 = -i? (G.7) 
apply Lagrange's equation for each generalized coordinate: 

(Jf + + /r)ft + + /r*i = -7* (G J) 

where T R is the torque developed between the rotor and it's stater which is fixed 
to the unicyde frame. 

/itCft + &) « A + 7* (G.9) 

W 

/Kft + £0 = -n\ (G.10) 

Eliminate the Lagrange multipliers by substituting G.9 into G.10: 

( Jf + nj ft )^ + aJafc + / x fc = nr* (G.ll) 

use the constraint equation to eliminate ft from equations G.S and G.ll to yield 
the two dynamic equations of motion: 

Uf +Jr + J T )qi + (Jt + nJ R )qz = -7* 
or in items of the system coordinates: 

(^r + Jii + Jr^ + ^r + n/^^-r/i (G.12) 



and 



or in terms of the system coordinate* 

+ + + (G13) 

-can useeouationG.13 to eliminate ^fc.^ G , 2; walchth<a ^ 
[Jr + (I - .y^ + n(1 _ n)jRil _ _ (n + ^ . c ^ 

E^ati o-GiaandG^arenowinaform^cncanoecom pared with eouaUons 
of the Uteral system derived in Appendix A, B and C where the mo" r 2 
memas were assumed to be zero. 

As we will see later, „ » a ^ (approdmately fof ^ 

^cyde parameters. We can therefore approximate the rieht h^d „<• 
tion G.14 by -„r«. ^ * 



Eliminate 7 between equation* G.13 and G.14: 

,7.- -(nJ„ + Jr\nT n 

Jr(J T + J F & - 2J T J R n + j^j p + j H j (G.15) 

To get maximum acceleration as a function of n let ** - 0 



Notice *'r - 00 i.e. load is held fixed 
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Also notice that equation G.16 can be approximated as follows: 

: ^ J* r 



Jt + Jf 



\Jt + JfJ 



JtJf 



small compared, to / \ small compared to / 
if Jr < ^t»«^f the optimal gear ratio is by good approximation: 

G.3 Yaw acceleration as a function of gear ratio 

Since the standard gear sizes allow a discrete number of gear ratios only, it is useful 
to plot equation G.15 as a function of the gear ratio, n. The CTRX-C program 
latnopt.ctr is used to calculate this function and the resulting plot is shown in 
Figure G.2. 
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TURNTABLE 




J T * moment of inertia of 
the turntable about 
its rotation axis 



GEAR TRAIN: total 
gear ratio = n:l 



- motor rotor angular rate 
relative to the frame 



MOTOR STATOR fixed to frame 



MOTOR ROTOR: 

H« moment of inertia of motor 
about its rotation axis 

FRAME 

j » moment of inertia of 
frame and wheel about 
its vertical axis 

WHEEL 



\j/ » frame yaw rate 



Figure G.l: Turntable Gear Drive Schematic 
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YAW ACCELERATION VS. GEAR RAT Id 

iOO. f n : i — — — — • 




0.0 10. 20. 30. 40. 50. 60. 70. 80. 90. 100. 



GEAR RAT 10 : N 

Figure G.2: Normalized Yaw Acceleration as a Function of the Turntable Drive System 
Gear Ratio 



Appendix K 

Servo Aceelerometer Design 



H.l Introduction 

Have an accuracy of annm^n>,u. — , r 

celerometers which measure to these accuses are prohibitively expensive and a 
custom made aceelerometer was designed for the unicyde robot. 



H.2 Mechanical design of servo aceelerometer 

consists of a pendulum suspended by means of a b^Wco Pper flexure. " Op- 
ucal pick-off of the position of the pendulum is accomplished by an infra-red 
batting diode on the p^dnlnm which ffluminates a set o, photodiodes fixed 



152 



H.3. FEEDBACK CONTROL SYSTEM DESIGN 
to the accelerometer housing. 
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Electromagnetic actuation of the pendulum is provided by a set of coils on the 
pendulum and a pair of permanent magnets pressed into the walls of the hous- 
ing. The physical size and weight of the pendulum was minimized to keep the 
undamped natural frequency of the pendulum much higher than the bandwidth 
of the unicyde robot (unicyde bandwidth is less than 1 Hz). 

H.3 Feedback control system design 

Figure H.2 shows the block diagram of the feedback control system. The pur- 
pose of the control system is to generate a current in the actuator coils which 
will reposition the light beam to the null position on the photo detectors during 
accelerations. If integral error feedback control is used as shown in the analysis in 
Figure H.2, the current in the coil is proportional to the acceleration experienced 
by the accelerometer as long as the frequency content of the acceleration motion 
is much lower than the dosed loop bandwidth of the servo accelerometer. 

H.4 Optical pick-off considerations 

A Siemens SFH405-2(Table H.l) infra-red b'ght-emitting diode (LED) was used 
as a light source The pendulum arm was specially designed to allow the LED to 
shine through a narrow slit of 75 /im (0.003 inches) in the bottom of the pendu- 
lum. The photo detector is a Siemens SFH204 four-quadrant silicon photodiode 
(Table H.2). The active units are only 12 pm apart and 100 pm wide and were 
connected in pairs to sense the beam deviation along one axis only. 
An important consideration for the choice of this detector as opposed to twr> sep- 
arate phototransistors, is the fact that aB the active areas are on the same piece 
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of silicon substrate, which provides the most similar behaviour of the differential 
active areas under tempera! are variations. 

The choice of a light beam width of 75 ,an for the 100 /an wide active area of 
the detector results in a maximum to minimum variation of the detector output 
current lb* a pendulum motion of only 75 /un. 



By taking the difference between the output currents from the two active areas, 
a reasonably linear relationship between pendulum position and detector output 
signal can be obtained, as shown in figure H J. The output currents from the 
photodiodes are in the order of tenths of a M and the light beamwidth should be 
chosen wide enough to generate enough photo current without flooding the whole 
active area. The distance from the light source to the detector was minimized by 
mechanical design for the same reason. 

Temperature drift and other forms of process noise cause errors in the servo ac- 
celerometer outputs which cannot be distinguished from actual accelerations. It 
is shown in the analysis of figure H.4 that the errors due to detector drift can 
be minimized by maximizing the sensitivity of the photo detector to pendulum 
position changes. Since one of the few parameters available to the designer to 
increase this sensitivity is by decreasing the fight beamwidth, it is a conflicting 
requirement to that stated in the previous paragraph. A compromise of making 
the fight beam approximately half as wide as the detector active area was chosen. 



In order to compensate for the non-uniformity of the light source and detector 
sensitivity, the total light received by all the active areas of the detector is regulated 
to be constant as described in the next section. 



H.S. DETECTOR ELECTRONICS AND SERVO AMPLIFIER DESIGN 
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H.5 Detector electronics and servo amplifier design 

Figure H.5 depicts the electronic circuit diagram for the servo accelerometer. A 
voltage proportional to the total light received by the photodiode detectors, is 
compared with a well stabilized 8V reference signal The difference is used to 
regulate the current to the LED in order to keep the total received light constant. 

A differential amplifier stage converts the difference in the photo currents to a volt- 
age which is propotional to the pendulum displacement. The photodiode package 
is mounted on a separate printed circuit board which can be repositioned mechani- 
cally to receive the light beam in the middle of the detector under zero acceleration 
conditions. Any remaining offset is corrected for electronically. 

The small signals from the photodiodes are susceptible to interference from stray 
electromagnetics (e.g. 6OH2 hum, switching amplifier noise, D.C. motor commu- 
tator noise, etc.). All the photodetector electronics are mounted on a minatur< 
tircnit board (Figure H.6) which is positioned on the side of the accelerometer as 
dose to the photodiodes as possible. The whole unit is packaged in a metal box 
at ground potential, which effectively eliminates any stray environmental electro- 
magnetic influences. 

The actuator coil has a 25 ft resistance and requires approximately 160 mA to 
reposition the pendulum to its original position when the accelerometer is turned 
on its side. A class A servo amplifier is included with feedback to provide the 
driving current without cross over distortion. 

H.6 Openloop frequency response and plant model 

The frequency respuurc from the cc2 currtsri uvuvrul voltage to vhe differential 
photodiode output was measured. The gain and phase responses are plotted in 
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Figure H.7 and it is seen that the plant ^nse compam weD to ^ <rf ^ fi 
damped second order system with: 



- 0.684 low frequency gain 

w« = 46.6 Hz andamped natural frequency 
( - 0.01 damping factor 

s J + 2f tp.« + 

The coU inductance measured 22 mH and together with the 250 coil resistance 
and 30ft series resistor, it forms another plant pole at 



This pole is at a much higher frequency than the control bandwidth of approxi- 
mately 2O0 rad/sec and is therefore ignored in the plant dynamic model. 

H.7 Compensation design 

The compensator is designed to increase the damping on the lightly aamped plant 
pole, and it also adds a pure integrator into the loop to provide zero error to 
contant acceleration inputs. Two additional uses are quired to draw the closed 
loop root locus far enough into the left half of the s-plane to provide an acceptably 
short settling tine. 

The second order electronic compensator that was selected is shown in Figure H.8. 
For unity feedback, the closed loop characteristic polynomial is: 



l+C7,O)<7 e O) = 0, 



H.7. COMPENSATION DESIGN 
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The desired dosed loop poles were chosen to be at the 4th order ITAE positions, 
therefore the desired dosed loop polynomial is: 

[( JL + 0 .424) a + (1^63) 3 ][(— + 0.626) 2 + (0.4141) 2 ] = 0 [B.2) 

where too is the desired bandwidth of the compensated dosed loop system. 



Simplifying equation fl.2 gives: 

s 4 + 2.1tPo3 3 + 3.4105^ + 2.7ta^ + w 4 , = 0 (E3) 

Solve for Ri by comparing the coefficients of the powers of s in equation H.l and 
equation H .3. 

* 2 = ^^f _l1 

wJK-Ri + + fill - rh%pR& {UJ>} 

41 = n^c < H7 > 

A condition that steins from the fact that the resistor values must all be positive, 
is: 

(Vo)mm > 0.5423t£7 n (BL8) 

A CTRL-C program, Igtitaextr' calculates the required resistor values. The re- 
sulting print-out in Section H.8 shows that the compensator results in a pole at 
s=0 t one more on the negative real axis and a pair of complex zeros in the left 
half of the s-plane. 
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A root locus of the dosed loop poles versus the overaD loop gain, K, a shown 
in Figure H.9. The poles are at the 1TAE positions for K=l. The compensated 
openloop system frequency response in Figure H.10 show, a 30 degree, phase 
margm. A low pass filter is added on the output to reduce the output oscillations 
dunng impulsive disturbances. The step response in Figure H.11 shows that the 

oosed loop system is reasonably well damped withasettlmgtimeofapproximatelv 
30 ms. 

H.8 ITAE compensator calculation program 



PUUTT PARAHETEBS 



0.6840 



46.6000 



ZETA 

0.0100 

VI 

292.7964 
PUTOOLES - 



-0.0293 ♦ 2.9278i 



H.8. JTAE COMPENSATOR CALCULATION PROGRAM 

-0.02*3 • 2.92781 

IT1E C0HT0BATOR PARAJCTEW: HH PUO C.L. BUDV ; CKTAPACnUE TILDE 

vo • 

351.3557 

c 

1.0000d-07 
EXACT RESISTOR T AIDES 
Rl 

2.83904*04 
R2 - 
1.617ld*0S 

R3 

2.749*H>3 

R4 

1.35534*04 

IEAKEST AVAILABLE RESISTOR VALUES ft CAPACITOR SHORT RESISTOR R5 
Rl - 

28700.0 
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R2 - 

ioivvw.w 

R3 

2870.0 
R4 - 

14333.0 

BS 

3300000.0 

CCKPCiU • 

US. 7491 

CGHFZERDS - 

1.0<HO2 • 

-0.8088 ♦ 1.94S41 
-0.8088 - 1.94S41 

CQKPPOIZS - 

-714.28S7 
-3.0303 

CLOSED LOOP STSTD! PA&AHEIEBS 
CIZEBOS • 
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1.04*42 • 

-0.8088 ♦ 1.94641 
-0.8088 - 1.94541 

CLPOLES • 

i.0d*02 • 

-1.5542 ♦ 4.54071 

-1.5542 - 4.54071 

-2.0617 ♦ 1.43611 

-2.0617 - 1.48611 

LOV PASS FTLTEK 01 OUTPOT ( VC • CUT-OFF FREQ. H EAO/SEC ) : 

vc 

212.8000 



H.9 Servo accelerometer calibration 

The servo accelerometers were mounted on a dividing head for calibration, using 
the gravity acceleration and tilt angle relationship to calculate the acceleration 
input. The dividing head has a 0.1 degree accuracy. 

First the servo loop was opened by disconnecting the actuator coil and the ac- 
celerometer was positioned vertically (0 m/5 2 acceleration input). The optical 
detector screws were loosened and the detector was shifted until the differential 
current in the two halves of the detector was zero. The dividing head was then 
tilted from -180 degrees to +180 degrees and the differential output voltage of the 
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optica, detector stage was recorded. Figure H.12 shows a reasonably W range 
with a steep slope for tilt angle, between ±20 degrees. 

Thereafter the servo loop on the lateral acceierometer was closed and the sensor 
was mounted verically on the dividing head. The oSset on the ontpnt voltage 

A very Uncar relationship „ obtained for a range of ±20 degrees. A least squares 
fit to the data point for the linear region yields: 

v ^t T = -0.311* voli 

where (ate k lte lateral Kceteralioo. 

Kif 7 ' = -0.3452 x 9 (volt) 
where « « the pitch angle in degrea. 

/jrtocc = -4L3l57oV*? r m / 3 2 
where IgUcc is the longitndinal acceleration. 



TW «« ** „„ „ topmnu , rfeea meiamd ^ ^ 

*7" V - ™" * "™ Wta " to ** — « » .racmrat, of 

± » de S ree «i the measured tilt angle. 
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H.10 Hall-effect frame angle sensor design 

A device to measure the unicyde frame pitch angle was designed by using a trailer 
wheel that touches the ground, as shown in Figure 4.1. A Hall-effect transducer 
(91SS12-2 LOHET by MICRO SWITCH, a Honeywell Division) is attached to the 
unicyde frame, separated from the magnet at the end of the aluminum rod -from 
the trailer wheel by a 2 mm air gap. 

The analog electronic interlace to the LOHET is shown in Figure H.15. The output 
of the fourth operational amplifier is connected to the analog-to-digital interface 
card described in Appendix J. 

The sensor was calibrated to determine the output voltage as a function of the 
pitch angle. The calibration curve is shown in Figure H. 16. The sensor is linear 
over a range of -5 degrees to +5 degrees with a gain of 03541 volt/degree. 



164 



APPENDIX H. SERVO ACCELEROMETER DESK N 




CU LEAF 
SPRING 



e.o. 



PHOTO OIOOES 
COIL BOBBIN 
SIDE VTpui 

DIMENSIONS ; lNCHES 



SCALE 2; J 



Fignre E.l: Mechaaical Deign of Servo Accekrca** 



H.10. HALL-EFFECT FRAME ANGLE SENSOR DESIGN 



165 



X 
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input 
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dynamics 
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P 
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e 



compensator 

dynamics 

G (s) 
£- 



K (s +as+b) 

c 



s (s+p) 



control 



current 



closed loop transfer function : 



G ( S ) - Icoil K 2 KpK c w^ (s 2 +as+b) 



s<s+p>(s* + 2Cw n s+w^) + KTK K<;W 2 (s 2 +as+b) 



for frequencies much lower than the servo acceierometer 
closed loop bandwidth: 



lim G a (s> 
s*>0 



= I coil 



approx 
steady 
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K, 



K, 



the coil current is 
proportional to the acceleration 



Figure H.2: Feedback Control System Block Diagram 
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x * beam displacement 




detector B? 



B ..« beam width 
* — — — i *4 

B - d 



^hbto sensitive area\v\ 





detector A^s 
sNSphoto sensitive area>^ 

^^^^^^ 




lineax range 
- B - d 



Slope S =— 
B 

—Optical drtectcr 
output = current 
of detector A - 
current of B 



Figure H .3: Relationship between Differential Photodiode Output and Light Beam Po- 
sition 
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pendulum 
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\ - tosses on pendulum arm 



e \ 




% x ■ displacement approximately proportional 
to deviation angle 9 for small angles 



x = linear acceleration 

_L 



IK 



Q " + 



D (detector drift) 



Q spring constant 
a 




K, 



B 



HO 



A v 



coil actuation current 



= S 

- optical 
detector gain 



n 



^coii 



X 



. is 

W 2 K s 



loopgain 



and 



D 



loopgain 



x — 



Av x loopgain = D 



• • 


K 5 


X 




D 


K 2 1S 



to get the apparent acceleration due to drift 
D, small, make S(=2M) as large as possible 

make R as small a* pn^Hip 

Figure H.4: Sensitivity to Process Noise Analysis 
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Figure H.6: Opto Electronics Printed Circuit Board Layout 
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Figure HA Second Order Electronic Compensator 
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Figure H.9: Closed Loop System Root Locos versos Overall Loop Gain K 
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Figure H.10: Servo Accefexometer Openloop Frequency R«pons« 
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Fig«e H.11: Theoretical, Step Respoiue of the Cloeed Loop 
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LATERAL ACCEL. 0PTICS XFER RJNCTI0N 
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Figu fl H - 12: Differentiai Outpnt of the Optical Detector as a Function of the Accelerom- 
eter Tilt Angle 
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LATERAL SERV0- ACCEL. CL0SED L00P XFER FUNCTI0N 
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Figure H.13: Measured Transfer Function for the Lateral Accekrometer 
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Figure H.14: Measured Transfer Function for the Longitudinal Accekrometer 
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Figure H.15: Hall-effect Transducer Electronic Interface 
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L0NGIT. PITCH 5ENS0R TRANSFER FUNCTI0N 
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Figure H.16: Measured Transfer Function of the Hafl-effeet Transducer Pitch Angle 
Sensor 
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SIEMENS SFH 405 SERIES 

INFRARED EMITTER 




Table H.1: Siemens SFH405-2 
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SILICON FOUR QUADRANT PHOTOOIODE 




T»ble R2: Siemens SFH204 



Appendix I 



Position Encoder and Servo 
Amplifier Interface 

1.1 Introduction 

This computer card extracts velocity information from the position encoders mounted 
on the axies of the turntable and wheel drive motors, and interfaces these signals 
to the G64 bus of the GESPAC computer. It also takes digital motor current 
command signals from the computer bus and outputs it as analog signals to the 
servo amplifiers. 

1.2 Position encoder interface 

The Datametrics K3DO-20G-5SE-4A optical encoder outputs two 0 - 5 V square 
waves in quadrature at 200 pulses per revolution of the motor shaft. On the state 
transition of one of these signals, a counter is started. It is dodced with a high 
frequency dock derived from the microprocessor system dock. When the next 
encoder state transition occurs, the counter's contents is stored in a register, the 
counter deared and the process repeated. The number stored in the register can be 
read by the microprocessor and inverted to obtain motorshaft speed. A schematic 

181 
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INTERFACE 



diagram of the position encoder digital logic is shown in Figure Ll and Figure L2. 



13 Operation of the logic circuitry 

A master clock, SCLX, controls the timing of all the logic transition, of the pc 
s.Uoa encoder interlace card, a, .now. in the timing diagram of Figure L4 The 
frequency of SCLK is jumper selectable, and is chosen from consideration, dis- 
cussed in the next section. The two quadrature signal, from the position encoder 
« synchronised with SCLK and the direction of rotation i, determined and indi- 
cted by the 0/D. The logic level of this signal is written to the data bus bit zero 
«• indicate the direction of motor rotation to the microprocessor. When the mo- 
tor stands still or reverses direction hexadecimal FFFF is written to the counter 
buffer register in the 74LS699 ICs by means of the CTjbUB strobe. 



One clock cycle after the low to high transition of the second phase signal of the 
Position encoder, the upper 15 bit. of the counter are loaded into the buffer reg~- 

dock cycle later, the counter is cleared (^strobe) and it starts counting the 
number of dock pulses until the next low to high transition of the second phase 
of the position encoder occurs. 



The lower 5 bits of the address bus is decoded to place the lateral oosition encoder 
rate aata at peripheral address hexadecimal 800020 and the longitudinal encoder 
rate data at 800030. 
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1.4 Selection of the interface clock frequencies 

Define: 

c = the number of clock poises counted daring one complete cycle of the second 
phase of the position encoder. 

f = the frequency^ Hz) of the master clock of the position encoder logic circuitry. 
u> = motor axle angular velocity in rad/sec 

For the encoders which generate 200 pulses per revolution, the motor speed is 
given by: 

2*7 

W= 200c* ^ /seC fl-D 

From the lateral control system simulations, the maximum turntable speed is 
expected to be = .12 rad/sec With the turntable drive gear ratio of 60:1 
the maximum lateral drive motor shaft speed will be 60 x 0.12 = 72 rad/sec. 
Let the counter on the interface card reach its maximum count of (2 ,J - 1) at 
1% of this speed. The lateral encoder interface clock frequency is calculated from 
equation Ll as: 

f = 

1 2x 

200 x O.072 x (2" - 1) 
2x 

= 75-096JfcJTz (L2) 

since the clock is derived from a subdivision of the 8MHz microprocessor system 
dock, a convenient value is 

LATCLK = = 31.25*** (L3) 



The typical nominal wheel speed is fto = 3 rad/sec The typical maximum 
turbation of the nominal wheel speed is ft = 0.12 rad/sec 



.8, ™«< ™^«« ws£mtoI!SIman(I 

Tie gtai ratio for the wheel drive < 



wheel speed is: 



• system is 2*1, so the motor speed at 3 rad/sec 



^ 3X24 = S£ (L4) 
Kthe accuracy of the motor speed shouid be measured to » of the maximum 
perturbation speed, tiea: ™ 

100 200(c-lj (L5) 
"» 200(c-l) (16) 

equate c between equation L4 and L6 to get the optimum clock frequency 

f = 200^(100^4.^) 

2tw "~ (L7) 

= 5.732Af^z ,. M 

(L8) 

u. a. ^ ^ ^ 

unciK^ma, (L9) 
I S Servo amplifier interface 

Th« „ DC motra . ^ pute ^ modntwi (pwM) ^ 

riv uw i, " e^M, — 

z jz mp ^ ™ * — — — — - - 



t ^ — - - - »- * „ J! 
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Define: 



D - digital number supplied to the DAC 
B = digital number appearing on the microprocessor boa 
Vout - output voltage of the DAC 
Vref = reference voltage for the DAC out pat 
= 10V 

For the DAC and operational amplifiers as configured in Figure L5, 

D - 2048 

where 0 < D < 4095 

Because of the negative logic of the G64 data bus, a binary value of B written to 
the DAC will give an output voltage of 



V <~t = *W(^§4^) V ^t (111) 



where 0 < B < 2048 



The lower 5 bits of the address bus is decoded to place the lateral DAC at periph- 
eral address hexadecimal 800000 and the longitudinal DAC at 800010- 



i 
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AMPLIFIER INTERFACE 




Figure U: Podtka Encoder and Servo Amplifier Interface 



Card Layout 



190 APPENDIX L POSITION ENCODER AND SERVO AMPIJITERINtERFACE 
■ +5 V-r- 




G64 BUS 



Figure L5: Digital to Analog Converter Interface to Servo Amplifiers 



Appendix j 



Analog-to-Digital, Radio and 
FPP Interface 

J.l Analog-to-Digital interface design 
J. 1.1 Hardware design 

Figure J.l shows the AD574AJD 12 bit analog-to-digital converter(ADC) from 
Analog Devices Inc. and its interfacing to the G64 bus. Its peripheral address 
is S8000FO (where the S sign implies a hexadecimal number). Up to 8 analog 
signals with magnitudes between ±10V can be muliplexed (by the multiplexer 
at address S8000EO) and connected to the sample-and-hold device at peripheral 
address $800000. 

J.1.2 Software for controlling the ADC interface 

To read and digitize one analog input channel, the microprocessor executes the 
following commands: 

1. writes the code for the analog channel to be discretized to the multiplexer 
address S80OOE0. A binary number of 000 on the data bus will correspond to 
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APPENDIX J. ANALOG-tO-DIGITAL, RADIO AND FTP INTERFACE 
the first analog input .hid, is the lateral Aerometer signal The k* 
accelerometer signal ' ^ " »»«"udi»al 

"^•^-.iiNtt,^ the 
3. writes a T on data bus bit 0 to address *8000DO to start tne san^ 

of «e L^Asaxnple and hold device. *« 
4- enters a software wail stat€ for _ ... 

6. wntes any number to the ADC address at J8000FO t„ , . 

^conversion. at ^™ to start the a* a Jog to 

8- reads the digitized value of the analog input. 

>• ^^a)^(8)toreadtheotherana te gin P utsigna 1 s. 

The FORTH program code to implement this algorithm ■ , 

.ms ajgonthm is shown in section J.1.4 

J '" B ^^«« of the analog input voltage 

The 12 output lines of the ADC are connected to the 12 least ■ •« 
bits (L<;m «r *u 12 ieast significant 

115 t LM, J « the microprocessor bn* tk* ^ 

« . K wa>ear dus. The 4 most significant bits mrcn \ 

databns are maslrM ™* - ^ e^euw oits (MSB) of the 

The ADC null offset is trimmed so that th- • 

il 11 1 ii i . ^ C maj ° r ^ operation (binary 0111 

1111 to 1000 0000 0000 1 «k«»u 
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transition (from 1111 till 1110 to 1111 1111 1111) should occur for an analog 
value l\ LSB below the nominal full (9.9963 volts for 10.000 vdts full scale). 
Fitting a straight line through the above two pairs of coordinates gives the analog 
reading: 

Ain = 10-00366 - 4.884 x 10- 3 (4095 - D^) 
where is the 12 bit value read on the databus. 

J. 1.4 FORTH Program to drive the ADC converter 

( — Analog to Digital Coortxttr Driwr — ) 

ca 

OILT FORTH DETHmoiS 



HEX 800000 C0BTAIT LATDACAflOt ( Sit UT QIC's addraa ) 
800010 CDlSTlfT LBIDACASOt ( Srt LOT DAC's addraft* ) 
800020 CD 1ST AIT LATEKAflDt ( Sat UT Encodtr's addraaa ) 
800030 COISTAIT L0OTCADD* ( Sat LCT Eacodar'a addraaa > 



8OO0D0 COISTAiT SftSADDK ( Saapla ft fold's tddzttt ) 
8000EO COISTAIT HXADO* ( Rnltiplczar's add r—a ) 
8OO0P0 COISTAIT ADCADOR ( Analog- to-Digital Convartar'a 



0 COISTAIT LATACOm ( HDXCoda tor Utaral Accalaroaatar ) 

1 COISTAIT LCTACCWX ( HDXCoda lor Longitudinal Accalaroaatar ) 



ca 



DECIMAL 



.< Loading LATDAC word codt) a 

: LATDAC 10000 SVAP - 2048 • 100O0 /( gat 12 bit «qaiT. lor output to DAC ) 
LATDACADDR ¥! ; ( «rita 16 nit tord to DAC1 ) 



:an * l '-lfM mi .„ ltt . 

0 00 UWP ; 



< ****** OTWTCH •ort eod . , e, 

•*«^*c. m mJl to oottlo ) 



' C SAMPLE and BOLD nrn t l 

-C U.diag 1D.C0TOOQ .0* coa. ) c« 

9 WIT4I C lUlt, 36 mioKm^ ft- „v ~^ 

w ADC fifniii. \ 



-( U^Uag ItoD.CDITm .ord cod. ) CR 

*p.cpwHtna ; 



• ( lading BUTlCC^rt cod. ) CR 

: KUT1CC ( - Uticc.tolt ) ( Cobt iaml 

10003««o 4884 409$ ' **** ^ Ut * t » I ««rt ro ».t«) 

UTJCCMX AtoD.COIfm 
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: HUTICC ( - Ljtlcc.Tolt ) ( Cm. Haloj folt. tnw Lw^it. Acc«l«xtwt«r) 

10003660 4884 4096 

LGTJCOCX AtoO.COITEXr 

- • - 1000 / . ; ( Output in xilliiolt* ) 



J. 2 Radio receiver interface design 
J.2.1 Hardware design 

The radio receiver emit* pulse width modulated (PWM) signals in response to 
the radio signals received from the radio transmitter. The pulses for each radio 
channel are repeated every 17.4 ms and has a low voltage of OV and a high voltage 
of 4V. The pulse width is 0.82 ms for a ma^mum negative stick input and 1.72 
ms for a mayimnm positive stkk input. 

Figure J 2 shows how the 8254 programmable interval timer (PIT) is used to count 
the number of timer dock pulses during the high level of the PWM signal. The 
timer interface clock is 4 MHz, half of the microprocessor clock frequency. The 
count for the maximum positive stick input will therefore be 7280 while the min- 
imum ccTiiit for full negative stick will be 3280 counts. 

The PIT has 3 independently programmable 16 bit counters. The PWM output 
frotn channel 1 of the radio receiver is connected to the gate of counter 1 at address 
$800082. The PWM output of channel 2 is connected to counter 2 at $800084. 

J. 2.2 Software interface 

Counters are programmed by writing a control word to address $800086. Coun- 
ters 1 and 2 are programmed to operate in Mode 0, by sequentially writing the 



^"^ M "^^^~«^ Because o*tlw 

^ ^ * 01 ^ — * — % 

-J. by tie nucroproeenor, nod, 1ST and MF. 

to generate an interrupt request (IRQ) to the microprocessor. 

111 * v *''*^'* fct the lateral and longitudinal input cottunaads The 
countersare theni^jjiitiaJized to^mh^i^.h. • 

P LT inters. m ■ **** - *i 

A of the software code that drives the ^ ^ ^ fc ^ fc 

the next section. The interrupt service routine, RADIO-EEAD V 
Motorola 68000 assent ^ «» had.o READ, was written in 

W5WU "»«nWer code to maximize the aiw~i »k • 
rontine. the speed of the interrupt service 



routine. 

Program to drive the radio 



CR 



oilt pdktb oemmois 

ALSO 



receiver interface 



VARIABLE U1PSH 
VARIABLE LST7VH 
BEX 



^ 00151111 < IU* e IUc im i^^ , 

CMSTAIT BCCOOTIBU ( Cox«t«r Wr^, ) 
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800084 COBTilT RCC0OTTE82 

800085 COBTUT ftCCOtTOLWW) ( Comrtar Controller* iddmt ) 
800090 CMSTUT BCTRXOCZ& ( Tri«nr addrsss to RESET ISQ flip-flop ) 
S8 COISTAIT IZf FT^Tt q < bml2 Antoractor addrsss ) 

KCTUL 



CODE UDIOJEAD ( Road* Radio BkiItv Channals upon IRQ ) 
BDVE.L D0,-(A7) ( mim contents of DO on stack ) 



KJTE.B 8t21.ICC0alR0UlGBD ( Latcaas all counts ) 

IDTE.S aCCOOTim.DO ( Roads LSB of Coontsrl ) 
BDVE.B DO.UTPW! ( SaT« toBjxxmly ) 

1OTE.W RCCOUItOU.DO ( Bands RSB of Cotmtorl ) 
LSL.W 88. DO 

WVE.B UTPWI P D0 ( Gat LSB ) 
W*E.¥ DO, Limn ( San ) 

HOTE.V BCCOOTim.DO ( Roads LSB of Cowrtort ) 
KJVE.B DO.LCrmni ( Saw tasjpoxaly ) 

1DTE.V RCCOOTTDB.DO ( Raads RSB of Countar2 ) 
LSL.y 83. DO 

HDYE.B LCTPW1,D0 ( Gat LSB ) 
DO.LCTPW < Savs > 

HDVE.B 8S8F.RCC01T10LBDM) ( Bsnritas Cntrlvord to 1 ) 
WtE.B •O.BCCODITnU ( Brlta LSB of initial conntarl ) 
WVE.B •O.RCCOOTTHU ( trita RSB of initial cotmtorl ) 



198 



APPENDIX J. ANALOG-TO-DIGITAL, RADIO AND FPP INTERFACE 
"we.b mr.Kcomrimo { to 2) 

».BOTTOS2 ( fait. UB of iaiti*! eowrt ) 

*"e.b w.accmmaa ( unt, rss if i^ti* CKatmx3 , 

"° TC - L ( * 7) *-°° < or*!** «i„ n, , 

WE ( Baton fro. Mcption rout la. ) 

ED-COOE 

CH. 

* Lo^ co- to i.atuxix. a/co^z , lBUlxvt , 



: MBIOJKT ( I-iiUlixo B*iU Coi*«,i t_ . 

"•"**- CJtroI Inter! *co t Intotrspt ) 

C'J R4M0.HEAD ( InitUlix. Udio ^. iymT , 



BHAHT 

10001111 McoramsoB) c. ( s. t c«t«, Ui , 

010011U BCCOnUUIDBO C! ( tor Hod. o ) 

1 BCTRZGGER C. (a^npfli^,,^^, 
DECIHAL ; 



: HlGaBIITJIE C Wait a > 
10000 0 00 10OO 1000 • Dffljp LCQP ; 
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CR 

. ( Loading BCS80V cod* to *bo« R/C U*dii* on T«xsisal Saw ) 

CR 

: BCSHDV ( Show R/C toadiag on trainal Scx*«a ) 
BBCZI. 

L4TPVH Vt S1SA * CR 20 SPACES . 
5178 LGTFIffl W - 10 SPACES . 
VlGnBXETJIZ TTERKHil 
JillL ; 



J.3 Floating point processor (FPP) interface 

The Motorola MC68881 floating point coprocessor interface to the G-64 bus and 
MC68000 CPU is shown in Figure J.4. 

The address decoding locates the base address for the FTP at $20 000 in memory. 

The Mach 2 FORTH Software contains a special floating point instruction set to 
utilize the floating point processor. 

The component layout for interface card described in this appendix is shown in 
Figure J.5. 

J .4 Electrical and electronic wiring diagram 

Figure J. 6 shows schematically how the electrical and electronic elements on the 
uxiicycfe robot are interconnected. 
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JT12V 



G64-BUS 




.J74LS74 



Figure J.1: 12 Bit AiuIog^DigiUl Converter and 8 CWei Multiplexer Interface 




Figure ]2: 8254 Programmable Interval Timer Interface 
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Figure J 3: Radio Receiver Interface Tuning Diagram 
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Figure J.4: Floating Point Processor Interfece to the G-64 bus and MC68000 CPU 
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Figure J .6: Electrical and Electronic Wiring Diagram 
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Gear Ratio for Maximum 
Wheel Acceleration 



K.1 Introduction 

Tie uaicycfe wheel is driven by » dir-rt 

KearandH-l^ • m ° t0r m ° Mtcd ° a ^ <W A 

^dbeiMnves^^,,^^^ 

forward cr ^ away ^ ■ 

"«im«a acceleration of the wheel to L * ^ 

the frame's center of ^ n ^ * ^ ^ W~ 

to allow maximum acceleration of the wheel 

K.2 Opti^ gear ratio for wheddri^s^tem 

s«. dm. SJTO „ h,, «,« ^ 

""<' i -'» (K.1) 
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Assuming no slip at the ground contact point of the wheel, the second constraint 
equation is: 

* = »vn (K.2) 

The rotational and translational kinetic energy elements of the system art- 
Wheel: 

TZ = : ( K.3) 
if = \mw# (K.4) 



Frame: 



Motor rotor 



ft =fi ; ft =fl 
ft =x ; ft =i 
93 =8 ; ft =tf 



(K.5) 
(K.6) 



rj?=j4W*) J (K.7) 



The potential energy of the system is: 

V = m ft F g cos 6 (K.8) 

The Lagrangian is then: 

L = Ttot - V'tej 

+ I?{u + 9?]-mrT F gzQ!i6 (K.9) 
Let the derivatives of the generalized coordinates of the system be: 



(K.10) 
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The two nonhofonomic constraint equations K.1 and K.2 ate augmented to La- 
grange's equation to yye: 

dfdi\ ai » ( 

(for i = 1,2, 3,4) 

where <?, is a generalized force. Friction forces wffl be neglected for the purposes 



of this derivation. 



l » = dGT) (K.12) 



(for j= 1,2) 

where 4>j are the constraint equations of the system. 



*i =w + n(0-ft) - o (K.13) 

(K.14) 



Rewritten in differential form: 

+ n(d$ - d&) = o (K.15) 
IJx - rwdSl = 0 (K.16) 

We can readily identify the a j{ coefficients: 

«u = -n <2 12 =0 a l3 = n a l4 =1 

«21 = 022 =1 023 =0 fi 24 =0 

Apply Lagrange's equations for each generalized coordinate: 
1- qi = 6: 

2. = x: 

(m;y + m f )i + mprpd = A 2 (K.19) 



(K.17) 



(K.18) 
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3. q 3 = *: 

( if + mfr^ + lf)$ + mprf-x + IjCj - mfr F g sin 8 = nA, - Q R ( K.20) 

4. q« = u: 

I?0 + 1?C> = \i+Qr (K.2I) 
where Qr is the motor rotor torque. 

Eliminate the two Lagrange multipliers to yield the two dynamic equations of 
motion for the system: 

[if + mFVpirw + r F ) + 1*)9 + [m w rw + m^iv + r F )Ji 
+ I?v+ I?il - mprpgshid = -Q R (K.22) 

(nlf + m F r r rMr)#+ n/f w + I?il + (mwr + m F )rwi = n<2/i 

(K.23) 

Use the constraint equations to eliminate the w and x variables: 

[if + mFTfirw + rp) + (1 - n)lf]8 
+ [^T + + mrrMr(rMr + r F ) + »/f ]« - mf-r/rpsin * 

[n(l - n)I* + mFrfrwP ]ff + + „J/« + (mwr + m F )rlr]n = 

(K.25) 

Subtract equation K.25 from K.24 

[If + mf-rj. + (n - l) a jfjf + [n(l - n)lf + mFrffMrjn 
= m F T F9 sm0-(n + l)Q R (K.26) 



= -<3h 

(K.24) 



Notice that equation K.25 and K.25 compare weU to tb» I • 
eooations of motion if the reflect 

^ ^ rf - tahen into 

Now proceed to calculate the optimal gear -atio r„, m • 

gear ratio for manmom wheel acceleration; 

Eliminate 8 between eqnation K.25 and K.26: 
Define: 



A = »(l-n)/* +TOrrrn|f 



^ - »>*Fr F $8in0 



(K.27) 
(K.28) 
(K.29) 
(KJO) 



6. HC + A) + A)Qo- *n 

BC-A* (KJ1) 

— a. - w „ ootany ^ , . , ^ » - 

We can set : 

dn dn 1 BC + A*~~\ Q * = ° (K.32) 
a»d solve for the optimal gear ratio analytically. 



The gear ratio nsed in the mechanical design,™ , 

-reuiimcai design may not necessarily b* ,k 
same as the optimal eear r*t;««i i . , . X the 

punuu gear ratio calcnlated above. It is therefore more n**,! >~ 

eqnationKJltoplotthecnrveof A M af„ „• ,T 
to see the effect of - 51 fMCt, ° n ° f », - order 

CffeCt rf n «ao P umal gear ratios nsed in practice 
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Equations K.28 through K.30 can be readily extended to include the mass and 
inertia properties of the turntable on top of the frame: 

A = n(l - n)I* -h {rtiftp + mrrr)rw 
B = . I? +n 2 r? + (mw + mF + mT)rw 
C = /f + mjr4+/f + m r 4 + (n--l) 2 /* 

The measured mechanical parameters for the unicyde from Appendix M and the 
CTRL-C program lgtnoptxtr is used to generate the curve of Figure K.2. 
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mass = m F 
moment of inertia 
about center of mass 
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inertia = I* 

(0 = rotor angular velocity 
relative to the frame 
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gear ratio = n:i 
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W 
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Fipue K.1: Wh«J Drive System 
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Figure K2: Normalued Wheel Acceleration at a Function of the Wheel Drive System 
Gear Ratio 



Appendix L 

Unicycle Mechanical Design 



L.l Introduction 



*W « th, mueyem ^ t6mfo « ^ ^ ^ 

mucycJe. 



A r-. OSes fonr major motions to balance on a oakyde. Tie Imtf^dma, 
st-sbfluation is largely obtained through the torques exerted 011 the pedals of the 

" * ^ ^' ^ ^ ^ motion «,,.. 
k *« m^cal mbas „« ^ ^ 

I""™ n °'" * W * * *-* ,y^ Ut. 

~m h. fc. „. „. _ ^ ^ ^ ^ ' 

^ ~ inertia of the upper part of his body to rap** th « ^ 

compon,.. of tt. ^ m^o. „«, on th, .bam „ .sed » nprfgh, *, 
^icyda I, „ sh™ in App^ p tlu „ ^ fc 

mom uoctjve than the sideways leaning motion for lateral stabilization of tha 
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Ll. INTRODUCTION 

unicyde. The rotary battery pack on top of the unicyde robot is used to simulate 
the twisting motion of a human rider s torso. It should be noted that although 
the battery pack is designed to allow fuB 360 degree rotations, it will not be used 
as a control moment gyro for stabilizing the unicyde. 



The mass, inertias and length dimensions of the unicyde robot were selected to 
be similar to those of a teenage child riding an actual unicyde. This will enable 
interesting comparisons between the computer stabilized and human stabilized 
unicycles without the need of scaling the time constants of the dynamic system. 

The unicyde robot consists of three major parts: 



1. The whed drive system (Figure L.l) 

2. Frame and dectronics system: This is the middle section of the robot and 
consists of a frame which connects the whed drive system to the turntable 
drive system. The on board microprocessor, servo amplifiers and other elec- 
tronic components are mounted to the frame 

•3. The turntable drive system (Figure L.2) 

All of the more complex parts of the unicyde robot were manufactured out of 
Aluminum (Al 6061 - T6) using a numerically coded milling machine. 



Full scale drawings prepared on the Hewlett-Packard "HPDRAFT" system are 
available on floppy disc at the Computer Aided Desigh Facility of the Design 
Division, Department of Mechanical Engineering, Stanford University. The Pascal 
code for programming the MATSUURA milling machine is also available at the 
same facility. 
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L.2 Effect of center of mass height 

Here, we simplify the unicycle model to consist of a wheel and a frame only. We 
neglect fricton effects and assume that a direct drive torque motor has its rotor 
on the wheel axle and its stator coni«ct*d to ih* frame. 

The longitudinal system dynamic equations can be obtained from equations N.l 
and N.2 by letting fif = m T = r,. = f w = 0 . We will be investigating the control 
torqne required to cause a constant wheel acceleration. Since we are interested in 
performance near t he state where the frame is stabilized vertically, 9 can also be 
set equal to zero in equation N.l. 



{if + mrrjryj + mprprwtl = -Q w 
kftftw* + \ff + { mw + m F )r^]d = Q w 

Eliminate 9: 



(L.2) 



9HL lj£ ± fggjjry + (raw + m F )rl,\ - (m^f-r^ 

= I IyL t ( mw ± m F)rw\ + Iff + mtrr^Imprj. 

™FTp + mfTwrp + If v--2) 

The expression on the right hand side can be viewed as the effective inertia that 
has to be accelerated by the wheel torque motor. 



Figure LJ shows how the effective moment of inertia decreases as the center of 
mass of the frame is placed at increasing heights above the wheel axle. 

Letting r F = 0 in equation L J give* the maximum effective inertia, which is the 
case where the frame center of mass is at the height of the wheel axle: 



Imas ~ ff + (% + !7»f)rJ 



w (L.4) 
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Letting rp — oo the minimum effective inertia is obtained: 

/mm = + mwrrk (L.5) 

It is therefore concluded that the mechanical design should place the center of mass 
of the frame as high above the wheel axle as possible for ease of stabilization. The 
side support frames of the wheel drive system were manulactured on a numerical 
milling machine and lightened out to leave a rib structure with minimal weight 
around the lower sections. The heavy drive motors were mounted as high on the 
structure as was practical. The turn table drive system which contains the lead-acid 
batteries is mounted at the top of the robot. Although the frame structural design 
was made as light as possible, it was designed strong enough so that structural 
flexibility effects need not be a consideration during the control system design. 

L.3 Design of the gear drive systems 

A precision cable and polyurethane chain drive system from BERG Inc. was se- 
lected for the gear drive systems. These chain and sprocket components require 
no lubrication and have small backlash effects at reasonable cost. It also facilitates 
gear ratio adjustment at any stage of the unicyde robots life cycle without the 
need for mechanical redesign. 

Bearings are supplied on gear shafts to reduce friction. The bearings are mounted 
in bearing blocks whose positions can be adjusted. In this way the drive belt 
tensions can be adjusted to minimize backlash in the drive systems. 

Appendices G and K show that optimal gear ratios can be selected for the wheel 
drive and turntable drive systems. The optimal turntable gear ratio is selected 
to yield maximum yaw acceleration of the frame, since this is the action that will 
be utilized to stabilize the unicyde iaterally. The wheel gear ratio is selected to 
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abom to ^ siflce th . . longitudinal 

instability wfl] be controDed. 



For the mechanical peters meMnred Md SOO wn m App€adjx M , ^ 

gear ratio for the wheel drive ^ is 24 : ! and 60 : 1 for the turntable drive 
system. 
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Figure L.l: Wheel Drive System 



220 
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elective; inertia' vs. -^a.ye c. v . -e:3-~ • 



< 
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Figure L J: Effective Longitudinal System Moment of Inertia as a Function of the Height 
of the Frame Center of Mass above the Wheel Axle 



Appendix M 



Mechanical Parameter 
Measurements 

M.l Inertia measurement method 

The moments of inertia of the wheel, Enju asd turntable about their mass ca- 
»rT 7" 1 ^ P - 4 ' ta — ■ *" *• 

The «o«*i„ ^ ^ „ „. ^ ^ ^ 

tat since the raamu « the ««. Th, MpIlt „ d « ■„ 

tbe^is^.^^^^^^^^^^^ 

angles t? and 7 shown in Figure M.l. 



VerticjJ force balance: 



2Tcos^2T = m g (M . 1} 
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Torque balance: 



From geometry: 



From equation M.l to M.3 



rBZh^ezLj (M.3) 



mar 2 

7 + -^-7 = 0 (M.4) 

The solution of this differential equation is asinusiodal function with an oscillation 
frequency of : 

myr 2 2x 




Wn = V IT = T (M * 5) 
where r is the period of the oscillation. With the mass of the object and the local 
gravity constant known, as well as the two lengths 1 and r, the moment of inertia 

is : 

M.2 Center of mass measurement method 

The center of mass for each object was determined by finding the locations where 
each object would balance when placed on a narrow beam. 



M.3 Friction measurements 

M.3.1 Longitudinal system friction coeffients 

The friction in the drive chains and at the contact point between the wheel and 
the ground are nonlinear functions of the rotational speeds. For simplicity it is 
assumed that the wheel drive system consists of a Coulomb friction and a visco-is 
friction component. An approximate value for the viscous friction was obtained 
by closing a velocity feed bade loop by means of the tachometer mounted on the 



The Coulomb friction in the wheel drive v/ttan woe determined k 

•Pride **, ^ ««„ * . hed * . ~ *—* * -»*«.•* 

the. ... ,^.,^22,^ 
Ccerionth ««to. „„,,, ^ to,, ^ « tt 



From 



W „ d ,he bowW ^ 



where: 



(M.7) 

(M.8) 
(M.9) 



m$. = ( mf r F + m T r T )g 
fc = coulomb friction measured in N - m 
»»A»ir. Wta M.7 can be Amplified to 

Laplace transform: 

^» r; (Mil) 

where So and * 0 is the initial angle and velocity rf th. «~ -„ 
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where 




(M.i3) 



The inverse Laplace transform of equation M.12 yields 

■«°*(**£)«^±£.. ■' . 
The amplitude of above time response decays linearily with ^ during every com- 
lete sinusoidal cycle which provides a means of obtaining the Coulomb friction, 
f ey from experimental data- 
in the present unicycle configuration where we actually measure 6(t), equation M.14 
can be differentiated to yield 

(±^-0o^w n smw n t (M.15) 
In this case the amplitude of the 8(t) response decays with 

^=^-»n (M.16) 

during each cycle. 

Notice that the undamped natural frequency w n can also be determined from the 
experimental data since the period of the oscillation is 




(M.iT) 



Figure M.2 shows a plot of the experimental data. By means of equation M.16 
above, it is calculated that the Coulomb friction for the wheel drive system is 

f c = 1.84 X - m (M.18) 
and the undamped natural frequency is 

u? n = 3.63 rad/sec 

= 0.58 hz (M.19) 
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M.3.2 Lateral system friction measurements 
The turntable friction is mea.ored b y dosing a velocity feedbaek loop m ^ 
turntable speed. The position encoder mooted on the turntable dlive motor 
shaft is used to obtain the speed of the Stable reiative to the frame. Various 
co^tant speed, around zero are commanded while the frame is constrained in 
movement and the average tor^ne over 100 sample, is calculated. When these 
data poinu are pitted with the whedspeed. on the ordinate and a straight line 
" fiWed ^ *"« ****** ^ of this fine yield, the vinous friction 
*nd the i»t«cept with the vertical axis give, the Coulomb friction pr«ent in the 
turntable drive system. 

The friction between the ground and the rubber tire i, not only very nonlinear 
but also varies with the type of the terrain that the unicyde is trav^ ing . M 
«timatiou of the ground friction during yaw motion, was obtained by a similar 
proeedu* as described above, but in this case the turntable instead of the frame 
was constrained from turning. 



SL4. MEASURED MECHANICAL PARAMETERS 

MA Measured mechanical parameters 

Wheel: 
raw = 2.109 *g 
r\v = 0.195 m 

I* = = 0.01888 kg . m 2 

1? - 0.03716 kg - m 2 

Iw = 0.047 N-m per r/s 

/c = 0.0245 N-m per r/s 

Frame: 

m F = 23.13 kg 

r r = 0.451 m 

/f= 1.3514 kg -m 2 

/ 2 r = 1.5121 kg - m 2 

/f = 0.3635 kg - m 2 



Turntable: 

m T =r 24.09 kg 

T = 0.797 m 

A T = /f =0.2928 kg . -2 

/f = 0.5028 kg - m 2 

h = 0.0786 N-m per r/s 

Lateral accelerometer height above wheel axle: 

r 53 = 0.186 m 

Longitudinal accelerometer height above wheel axle: 

r R3 = 0.635 m 

M.5 D.C.motor and gear drive system parameters 

Longitudinal System: 
Motor Rotor inenia: 
I? = 4.52 x 10^ kg . m 2 



D.CMOTORAND GEAR DRP/E SYSTEM PARAMETERS 
Motor Torque Constant: 

= 6.3 x 10~ 2 N - m /A 

Lateral System: 

»•* — — - ~- — «... 

= 4.377 x 10" s kg - m J 
Motor Torque Constant: 
A'f* r = 3.3 x 1<T 2 N - m /A 
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Figure NL1: Schematic Diagram of a Bifilar Pendulum for Measuring the Moment of 
Inertia of an Object 



MJ. D.CMOTOR AND GEAR DRIVE SYSTEM PARAMETERS 
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C0UL0MB FRICTI0N EXPERIMENT 




0. 1. 2. 3. 4. 5. 6. 7. 8. 9." 10. 11. 12. 13. 14. 15. 

time(sec) 



Figure M.2: Tachometer tad Accekrometer Rmpautt dozing Free Oscillation of the 
Upaidc Hown Unkyde 
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Continuous Time Longitudinal 
Control System 

N.l Longitudinal equations of motion 

/tt* 1 eq " tiOM ° fm0ti0Baa *- derive m Appea _ 

The foDo^ a-umption. «. ^ to deawp|e ^ 
t»ns from the lateral »y»tem dynamic equation* 



1. The nominal turntable angular rotation .peed * = 0 . 

2. The unicyde ^ a ^ ^ ^ 
that 1^ = ^ = 0. 

With these — Ptions, the longitudinal dynamic equation, of motion from equ* 
tions A^4 and A3S amplify to: 
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The derivations in Appendix A assume that the wheel it driven by a torque 

^ to drive the wheel The inertia of the d.c motor should be reflet to the 
wheel a. indicted by the dynamic equation, K^5 and for the geared drive 
.ynem fcrthewheeL The modified dynamic equation, of motion that include the 
finite motor rotor inertia /* are: 

^ + -^+^+^+(li-iW + K VF+lw) ^ +ll( .. 1)jWi 

The constant wheel .peed texxn, 0, do- not affect the longitudinal syrtem dy- 

aiBUC ' ^ th4a ». «W loss due to friction. In practice 

faction losses win U-vm^**,*^^.,.^ ^ ^ 

*m motor, by the con^t value. % k therefore ignored in the d«gn 

of the longitudinal feedback control system. 

Rewriting the,, equation, into state space reputation, they become 



Where: 



III 


Il2 


0 




0 




-fw 


fw 


As 




9 






In 


In 


0 




Q 




fw 


-fw 


0 




a 


+ 


i 


0 

■ 


0 


1 




8 

J 




1 


0 


0 

- 




e 




0 



Qw (N.5) 



Ai=/f +m r r> + /r + mTf ^ + (|| _ 1)2/ * 
7 " " 7 M = ( m FTF + mTrT)rnr + n(n - 
1*2 = 1? + (m w + m F + m T y w + n 2 I* 
-lis = (mpr F + mrrr)ff 



(N-6) 
(N.7) 
(NS) 
(N.9) 



234 APPENDIX N. CONTINUOUS TIME LONGITUDINAL CONTROL SYSTEM 

Multiplication of equation NJ> by the inveae of the first .natrix containing the 
inertia terms, yield, the standard state space form qf the system of dynamic equa- 
tions: 

x = Fx+Gu (1U0) 
where u = Q w and x - [j, ft, j]T 



N.2 Longitudinal sensors 

A tachometer and an accelerometer are used as the longitudinal 



The tachometer is mounted dir«tly on the rotor shaft of the wheel drive motor. It 
is actually a digital position encoder, but angular velocity information is extracted 
from this sensor by means of the electronic circuitry described in Appendix L If it 
is assumed that the wheel gear drive system has negligible elasticity and backlash, 
the tachometer on the motor shaft reads: 

"tor = «£cr(fto + n - 9) (N.li) 

where is the known wheel drive system gear ratio, fto is the constant nominal 
wheel speed and is typically much larger than the wheel perturbation velocity Q 
and the frame pitch rate B. Since fto is known, a measurement consisting of a 
combination ofthetwostatesQandjcanbe reconstructed in the microprocessor 
software: 

* = n ~ * (N.I2) 

The accelerometer design is described in Appendix H. It is mounted on the uni- 
cyde frame, with its acceleration sensitive axis in the direction of the forward 
motion of the unicycle. With the accelerometer mounted at a height r» above 
the wheel axle, it measures a combination of the acceleration due to the pitch of 
the frame and the forward acceleration of the wheel of the unkycle. As shown in 
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Figure *X the tignal measured by the accekrometer is proportional to the servo 
pendulum control torque: 

Q*bGT) = kjg$ - (rwtl + r»^)l (N.13) 

The pitch acceleration of the frame and the wheel acceleration are from equa- 
tion N.5: 

* = F X1 & + F l7 a + Fitf + ir (NJ4) 
Os/n^+Fan + i^ + Cja^ (N.15) 

By scaling in the microprocessor software and by using equations N.14 and N.15, 
the accelerometer measurement can be rewritten in terms of the states and control 
input: 

= HrKyFn+rwFnye-irjoFn + rwFTjyi 

+ (9 - rjtyF lz - rw - (r/ed + rwG 7 )Qw (N.16) 

N.3 Longitudinal system characteristics 

The CTRL-C program Igtchar.ctr' is used to nifrnlatt the longitudinal system 
characteristics. 

The results presented in section N.3.1 show the state transition matrix and the 
input and output matrices for the system with measured mechanical parameters 
as listed in Appendix IL The eigenvalues calculated show a pair of poles on the 
real axis on either side of the imaginary axis. These are due to the pitch instabil- 
ity of the frame of the unkyde. The third pole is the rigid body mode which is 
slightly shifted from the origin due to the wheel friction. 



The controllability matrix (marked CTR in the print-out) toons that all throe 

menu. me»*nr*- 
The ^ ^ ^ ^ ^ 

— ^^^^ 

u appnmj^t^y C.75 Hz. ^ 

NJ.1 Longit. ^Btem characteristic, calculation 



conm am . w^*. (Q¥) 



RR3 

0.6360 

run 



N.3. LONGTTUDINAL SYSTEM CHARACTERISTICS 



-0.0333 0.0333 54.8447 
0.1138 -0.1138 -151.3607 
1.0000 0. 0. 



GUT 



-0.7076 
2.4216 

0. 



HLOT 

'1.0CCC 1.5000 0. 
-0.0011 0.0011 4.6700 

■LOT 

0. 
-0.0230 

ZXCUL 

-0.0216 
7.3232 
-7. 



EICTEC • 

0.0000 -0.3620 -0.3692 
1.0000 1.0000 1.0000 
-4.000* -0.0*4 0.0482 

C OWBflt l l imT TT, Q8SQT18XLITT AID K3XKE 

cn • 



0.4697 
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0.9659 
0.9961 



1.0000 1.3*20 1.3592 

-00018 -0.2294 0.2257 

VSSZDO • 

0.4697 1.3166 1.3639 

-0.0008 -0.2216 0.2268 

nucm pwrnn mm tobl *xwe tobqte tq tj 

3.1291 



2.8309 
-2.8309 



PCttB 



-7.4 
7.3232 
-0.0216 



GUI 



MXVE TD1Q0C ID 



-0.0230 
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-0.0000 ♦ 9.44571 
-0.0000 - 9.44671 
0.0000 ♦ 0. 1 

POLS ■ 

-7.4486 

7.3232 
-0.0216 

N.3.2 Experimental confirmation transfer fdnctionj 

Before a control system for the longitudinal stabilization can be designed, experi- 
ments should be performed to verify that the actual plant transfer functions cor- 
respond to the theoretical modeL The unstable longitudinal dynamics are difficult 
to identify in an openbop configuration. The unicytle was therefore suspended 
upside down with the wheel clamped to a beam. Although this configuration can- 
not provide information on the influence of the wheel dynamics in the system, it 
can be used to confirm certain parameters of the longitudinal sys tem 
The theoretical model for the upside down unicytle configuration is 

Io* = -fw* - fc*gn(6) - mglB + Q w (N.17) 

where 

mgl = {mfT F + mTTT)g (N.18) 

Apart from Coulomb faction, f Cy the belt and sprocket wheel drive system also 
has other nonlinearities like backlash and torque variations due to the imperfect 
gear system. A tight velocity feedback loop was therefore closed first to enhance 
the system linearity: 
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Q w = -kr(0-y c ) 
so that the closed loop system become. 

If the velocity feedback gain » .arge enough the feedback to^ue will completely 
dominate the plant friction effect., .o tlut the Wer ronctio. can be appro* 
mated by * 

Henceforth the tachometer .peed command, can be considered a. the new 
longUudinal .ystem control inpnt. The Laplace tranrfbrm of thi. amnion pve. 

g(g) _ *r 

The state reputation of the continues time longitodinal ^ i, there- 
fore 

1' J [i o J[*j + [o J* ^ 

Th: accelerometer sensor meagre, the specific force on the frame at a Stance 
r fQ from the wheel axle: 

so that the dosed loop syste* transfer function to accelerameter measurement ia 

The frequency respond of thi. transfer function have the Mowing charactemtk. 
which can be verified experimentally: 

- The low frequency gain a 

- The high frequency gain is 

" ^ ^ W " = * » ti. Plant ahould have rero gain at this 
frequency. 
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- The undamped natural frequency of the poles are at w, s ±yffi. The 
relative position* of w, and on the frequency axis will be evident from the 
phase behavior of the experimental results. 

The proportional tachometer feedback compensator was implemented as a digital 
control system in the on board microprocessor of the unicycle A fast sampling 
frequency of 100 hz was selected and a computational delay of L2 ms between the 
sampling instant and the moment the control command is issued, was measured. 
This delay, T 4 , was included in the plant model by means of a S»t order Pade 
approximation: 

To include the delay in the state space model, an additional state, z 4 , is defined. 
Equation N.26 in the time domain then 



« = 2r,-« (N.27) 

The original plant > = F z + Gi of equation N.23, » therefore modified to include 
the transportation delay: 



z 



F 2G 
2 57 



-G 
i 

5? J 



(K58) 



A fisting of a CTRL-C program that calculates the discrete time system matri- 
ces for equation N.28 as well as the transfer function from the tachometer speed 
input command to the measured accekrometer output is shown in the next section. 



Sinusoidal tachometer speed commands were generated in the on board computer 
of the unicycle and used to excite the system. The accekrometer output signal was 
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^"i^iil^ta^i,^^ Notice 

«gn of the acceleration change.. Tib » probably ^ ^ backlash b the drive 
system, which cannot be coveted by feedback rince the tachometer k mounted 
on the drive motor and net diiectiy to the wheeL 

Figore N.4. how. the theoretical frequency respond with the meawred data point, 
indicated with .quart.. The .harp decrease in the gain between 0.6 and 0.7 hz 
confirm, that the trantfer function zero, are near the theo^tical location of 0.63 hz. 
The fact that the pha* tot decree below zero degree, and then ha. a pomtive 
Continuity indicate, that the undamped natural frequency of the com pi « pole 
pazr u, at . lower frequency (theoretically 0.58 hz) than the sere The mealed 
low and high frequency gain, are within 2 dB of the expected theoretical value. 
The d^pande. bet TO U>e me^ured and theoretical frequency responses are 
attributed to the following fecton: 

- The electronic internee u^d to extract angular velority information from 
the position encoder on the wheel drive moto, (Appendix 1) was designed 
to provide accurate wheel speed information in the vicinity of the nominal 

whed.peedof3rad/~- ^^^-encyre^identmcation experiment. 
w*h the wheel clamped to the suspending beam, the maximum value of the 
sum*** speed ==and wa, only 0 .05 rad/sec «, that the nation of the 
velocity informal** available to the controller, is suboptimaL 

- Characterirtk, of the belt drive syrtem like backlash and ela^ty I*ve been 
ignored in the W theoretical model. Although it u a»umed that the 
control torque donrinate. the friction efie^ 

soaaWe to expect that at low .pee*, frictknul characteri^ can be wome 
and include miction effect, when the speed become, zero at the apex* of the 
Pendulum rwiuo. 

In spite of these reservations, satisfactory agreement between the experimental 
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data and theoretical predictions has been obtained. The amchsion is dawn that 
the theoretical model for the np«de down pendulum is acceptable. Since the 
nnicyde parts with major weights and inertias as weO as the complete wheel drive 
system have been included in the experiment, it is assumed that the model for the 

vertical szicycic zz its Tried s 7s£j for pitch uau um t*. 

N.3.3 Upside down unicycle transfer functions 

ucydd/paad.ctx 

lot wm now acre «ra rise ipptax pat thk out 

LOMnTBJZIAL STiin : THKri.DOT; OHEOi; TWETl; ID 
UI11IOL ilWT : V2BEL MTTDt TCBQQB (QV) 




TSiHXXICI - 

0.0100 
TDEUTXid - 
0.0012 

C0STHUWB TDK StSTBI HATOCES 

FLBT 

1.0d*03 • 
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-0.0000 -0.0132 -0.0001 
0.0010 0. o. 

o. o. 



OCT 



-1.7094 



1.0**O3 • 

0.0000 
0. 



HLCT 

-1.0000 0. o. 
0.0014 -1.4440 0. 

o. 

0. 



DTSOEre TDK STSm BiTHCO 
PHI - 



-0.131? -0.0O01 
0.9993 -0.0000 
o. 0.0000 

GAH 

-O.0004 
-0.0000 
1.0000 



0.9993 
O.010O 
0. 



ftlMT nUBRft FDBCTXOB 
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-0.132S 
l.OOOC 



TACHPOLE3 • 

a /yyy> * ^ 4 
0.9993 ♦ 0.03d! 
0.9993 r 0.03C31 

TiCSSAII 

3.99S4H04 



TiABm racnoi nas bkzl dote 



TO 



-0.0000 ♦ 0. i 
0.9993 ♦ 0.03931 
0.9993 - 0.03931 

ACCPOtO • 

0.0000 ♦ 0. 1 
0.9993 ♦ 0.O3C31 
0.9993 - 0.O3C31 



accoaii 



0.0288 



FEOBACI LOOP 
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TACH LOOP PMP Q Pin iit 
IT 



GA2M7 



closed ticanem loop <un. 

TACHLOQPZZ - 

0.9?93 ♦ 0.03931 
0.9993 -0.03931 
-0.0000 ♦ 0. i 

TiCBLOGFPO • 

0.9937 ♦ 0.03681 
0.9937 - 0.035*1 
0.0013 ♦ 0. 1 

TJCHLOGPGA • 

0.7190 



wt» ; Tjcccsi/ncwiKs) 



N.4 Longitudinal control system design 

The feedback control system design » performed in three steps: 

opt*** feedback^ 

2. optimal W 

from the tachometer and aoderometer ineawrementa. 

3. Dzscxetize the continues time design so that it can be implemented 
digital control system in the znkroprocessor on the unicyde. 



as a 
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A block erf the feedback control svstem is shown in Figure NJ>. x i, the 

^vector [9 SI 0f and a k the control input torque, Q w . The meamred 
output vector, » the tachometer and accelerometer signals, ft m decided 
that the reference input, r, to the regulator should be a commanded perturb . V» 



the unicyde forward speed by external commands. The regulator also feature* 
integral error control to provide zero error to step mimt ccnunan* and improved 
rejection of constant disturbances on the plant 

N.4.1 Optimal continuous time regulator design 

If it is assumed that all the plan t states are available to the controller, an optimal- 
linear quadratic regulator can be designed to ininimire the cost function: 

/= Jj[°V* + « T *0* (N-29) 

where A and B are diagonal matrices whose diagonal elements are the weights on 
the states and control input. 



It was decided to introduce feedback of the integral of the error in the wheel 
spetd. This win ensure good control of the unkyefe forward speed and rejection 
of constant disturbance inputs on the plant. This introduces an extra state, e, 
and from the block diagram in Fjgure N4 the dosed loop system matrices are: 



c 




0 JET 




t 




0 




-1 












+ 








X 




0 F 




X 




G 

> 


0 



= -ic* C] 



(NJO) 



where H = [ 0 1 0 J for integral error feedback of the wheel speed ft. 

A CTEL-C program Tgtlqgxtr* is used to calculate the optimal feedback gains for 

specified weighting matrices A and B. A print-out of the result, of the program is 
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shown in section NAL The diagonals of the weight^ 

and BDIAG in the printout and the feedback gains are CERE and CLGT for the 
integral error and state feedback gains respectively. 



,!„ 



-1 1 l . ' ^ 



r ^ 



-GCo F-GC J [xj 



-1 
0 



«c 



ft = 



[0 0 10] 



(NJl) 
(N-32) 



If the state weighting matrix places a penalty, a, on the emr in the wheel speed 
only, and the control, u, is weighted by b = 1, a locn, of the closed loop pofc, as 
a function of f can be calculated. These dosed loop pole location, for value, of f 
ranging from 0 to 10 000 are plotted in Figure Nil. This is eoentiatiy a symmetric 
rootlocus plot. The low frequency branches of the loci approach the location of 
the zero and the reflected non minimum phase zero of £ as f approaches infinity. 
This constitutes an inherent bandwidth limitation for the unfcyck longitudinal 
closed loop dynamic, with a regulator demgned by the technique of minimizing a 
linear quadratic cost function. 



For practical re-on, thereof fshould not be too large, becau« the mtefeed- 
back gams would be so large thatrignal saturations m the controllei may occur. \ 
choice of fr = 30 gives moderate gain, and an acceptably ast dc^ 1«* r«mon^ 

Figure N.7 show, the time response of the closed loop system for a step command 
to increase the wheel speed by 1 r/s. 



Tie frequency response plot of Figure N3 shows that the dosed loop longitudinal 
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system has a bandwidth of approximately 0.3Hz. 
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N.5 Process and measurement noise models 

In order to evaluate the performance of various compensators, a model of the 
process and measurement noise is required. The plant model is modified to include 
these random disturbance effects: 

Vm = Mx + v (N-34) 

where: 

w u a random disturbance vector, with spectral density matrix Q 

v is a random measurement noise vector, with spectral density matrix ft. 



in 
in 



The complete statistical nature of these random noise signals is rarely known 
practice. Since we can usually rely on the inherent band limitation present 
most control systems, it is convenient to assume that w(t) and v(t) are indepen- 
dent white noise processes. We now proceed to estimate the ratios of the spectral 
densities. 



Let 
p. - 



-i - spectral density of the tachometer measurement noise in units of ratP/s. 
R* = spectral density of the accelerometer measurement none in units of m 2 /s 3 . 

From practical experience with the accekrometers and tachometers that were 
constructed, the ratio of the measurement spectral densities is estimated as 

Rt 0.05 jtj2i i 

rodV/m 2 (N.35) 

The principal source of process noise in the plant is the vibrations of the unicycle 
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caused by the roughness of the terrain over which the wheel travel* 

It is assumed that process noise enters the plant with the same input attribution 
matrix a. the control torque, Le. G n = G. In the absence of the exact knowledge 
of the statical natuie of the proem noise, it is a»unW that the spectral density 
of Q is much larger than the measurement noise spectral densities. Achofceof 
which places the estimator poles in the same s-plane region as the regulator 
poles is (see Section N.6.1). 

(NJC) 

For the numerical simulations in the sections that follow, it will be necessary to 
approximate the continuous time purely random gauss-markov protases (with 
the spectral densities Q and R given) by discrete time gauss-markov sequences 
(with covariance matrices QD and RD). Chapter 11 of reference [Brvson 2] show, 
that a good approximation of the continue 

gaussian random sequence with a correlation time T c which is short compared to 
the characteristic time constants of the plant, and a covariance (QD) determined 
from; 

Q.(t)~2-T e QD , N<37) 

likewise 



a 2-r e .iD, (1U9) 

The shortest characteristic time in the longitudinal system is about 0.125 see ( 
from the inverted pendulum pole, at s SS ±8 rad/s. A noise correlation time of 
T e = 04)1 sec is therefore assumed, and taken to be the same for afl 3 noise sources. 
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Even though only the ratios of the various spectral densities ate considered during 
an LQG estimator design, we need to estimate particular values for these quantities 
that can be used in time response simulations. From equation N.16 and the actual 
vines of the coefficients of the states, we see that the accderometer measurement 

If we assume that the spectral densities of the random noises are reasonably-uni- 
form from 0 hz to the control bandwidth, we can approximate the noise co variance 
by the square of the standard deviation of the signal, eg. 

RD.Zo* (N.41) 

Figure N.7 shows that the maximum pitch angle during a typical manuever is 1 
degree. The m ax im um acceleration is then (from equation N.40): 

(ya)m«2i0.1 m/s 2 (N.42) 
Aasume that (a.)^ is approximately 10% of (y.)^, then 

<r. = 0.01 m/s J ( N .43) 
RD. = (.01) J = 1 x 10- 4 m 3 /s* (N.44) 

From equations N.37 to N.39 the tachometer measurement noise covariance and 
standard deviating is 

RD t = 5X10" 5 raP/s 2 ( N .45) 
a t - y/RBl = 7.07 x 10~ 3 rod/ a (N.46) 
and the process noise covariance and standard deviation is 

QD = lxlO" 3 N'm 2 (N.47) 
<r p = y/QD = 3.16 x 10" 2 Nm (N.48) 

The computer language CTRL-C has a function that generates a random number 
with normal distribution (0 mean and standard deviation of 1.0) every time it is 



252 APPENDIX* CONTINUOUS TIME LONGITUDINAL CONTROL 



SYSTEM 



called. Simulation of the noise signal, will therefore be performed by using a time 
step equal to the 0.01 sec correlation time and then scaling the raudom number 
with the square root of the appropriate covariance. 

N.6 Optimal continuous time estimator design 

A linear quadratic estimator can be designed to ertimate all the state, of the lon- 
gitudinal sy«em from the accelerometer and tachometer measurements, provided 
that we have some knowledge of the degree of uncertainty in the measurements 
and of the degree of intensity of the raudom disturbance on the plant The olant 
model is modified to include the random disturbance effect, on the plant and 
measurements: 



i = Fx + Gu + G n w 

(N.49) 



where: 

x is the state vector 

w is a random disturbance vector 

y m is a vector of the measured quantities 

v is a random measurement noise vector. 

Assume that «*) and v(t) are independent white noise process with spectral 
density matrices Q and R, respectively. A Kalman-Bucy filter. [Kalman] with 
filter gains L, can then be designed for the optimal estimate of the state vector, 
i, given by 

*=Foi + G 0 v + L( 9m -Mi) (NJ0) 

Ideally the plant model used in the estimator would have the same parameter, ae 
the actual plant, Le. F 0 = F and Go = <?. 
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The complete statistical nature of the random plant disturbance vector, w, and 
the process noise input matrix, G+ % is rarely known in practice. By the lack of any 
better approximations, the process noise inpnt matrix is assumed to be the same 
as the control input matrix, Le G n = G. w is then a scalar white noise process, 
whose spectral density, Q, wffl be selected upon inspection of the symmetric root 
locus of the estimator poles. 

The spectral densities of the random noise in the tachometer and accelerometer 
measurements can be determined with frequency spectrum analysing instruments. 
If we assume that the spectral densities are reasonably uniform from 0 hz to the 
control bandwidth, we only need to concern ourselves with the ratio of the two 
measurement noise spectral densities during the estimator design. 

The paper by Bryson and Hall in [Leondes] give the Euler-Lagrange equations for 
the optimal filter problem as: 



X 




F -G n QGl 


A 




-M T BT X M -F T 





t 


+ 


0 




X 







Vm (N.51) 



w = -QGlX 



(N.52) 



If we take the Laplace transform of the above three equations, and eliminate x(s) 
and A(s) we obtain the symmetric rootlocus characteristic equation (SRCE) 



(N-53) 



where: 



Z t (s) 



= M(sI-F)- 1 G, 



(NJS4) 



- A"*™** coccus maicmmmAi COKTaot ^ 



If the two measurement noiw B «v^^ 

eat none processes are uncorrelated, equation N.53 can be 

rewritten as 



I *« + z — J =l 

If we let Z, i3 ) = we ^ ^ ^ 



(N.55) 



are a function of the ra- 



" *>(-«)/>(«) (N.S6) 

Tk« of tie ^mnmnc rot loc» «,„Uo« »bov« 



listed ui the next section. 

the low frequency estimator odes w«~. ;« *k 



™l M T*"" , *- M * 1 ""^t-"*lii— i -fesmui.. 
in tne pnnt-out. 



full 
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order estimator and feedback of the integral error and estimated states can be 
summarized as: 



(N.57) 
(N.58) 
(N.59) 



x = Fz + Gu + Gnw 
e - Hx-r 

i = foi + Cou + itym-Afi) (N.61) 

The interconnection of all these equations is shown in the longitudinal system 
blockdiagram of Figure N.5. These equations can be represented in matrix form 
for analysis and simulation purposes: 



X 




€ 




X 







X 




0 




G n 0 




c 


+ 


-1 


r+ 


0 0 




X 




0 




0 L 



F -GC C -GC 
0 0 H 
LM -GoCe Fo-GoC-LM ^ 

(N-62) 

The eigenvalues of the system matrix above are the same as the eigenvalues of the 
regulator and the estimator if F 0 = F, and G 0 = G. If the initial estimate of the 
state vector is the same as the initial state vector, the step response of this system 
will be the same as the response for the full state feedback system of Figure N.7. 
An example of the response when 0(0) = 0.1 rad,but the initial estimate 0(0) = 
0 rad is shown in Figure N.10. It can be seen that large excursions of the plant 
states occur initially because the control commands are issued based on the wrong 
estimates of the plant states. As the LQG estimator improves its estimates of the 
plant states, the wheel speed approaches the commanded value of Q c = 0 and the 
frame is held vertical (0(0) = 0 rad). 

The performance of the closed loop system in the presence of measurement and 
process noise was evaluated. The plot of Figure N.ll is generated with the initial 
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state estimate the same „ the initial plant state and noise inp n ts as d«cnbed in 
section N.5. 



N.6.1 LQE gain calculation program 

OCTCS/lGTLQa.CT* 

L0KUTDDII1L OTTTOL STSTEH U1B TOL OHO U> ESTXUTQft 



UOCITODIIAL STATES : THETA.DOT; QRBCi; THET1 

comoL am : vheel nnt& tdrqgc <qd 

«ASOiD!Em : TAOMCT&; ACOMMlRat 

uins : ieias, radius, secokb 

• awnraros tihe plot mtktces ~- . 

FLCT 

-00333 0.0333 54.5447 
0.113C -0.U38 -151.2607 
10000 0. o. 

GLGT 

-0.7075 
2.4216 
0. 

G»ISELCT - 
-0.7075 
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2.4216 

0. 

HLCT 

-1.0000 1.0000 
-0.0011 0.0011 

ILCT 

0. 
-0.0230 

IUTRICE5 FOE HTEGR1L EUOft FEEXBACK COITBOL t*M 

mnn» state tectoi is : OTBOi.m; ram. dot; acai ; theti 
fut • 



0. 


0. 


1.0000 


0. 


0. 


-O.0333 


0.0333 


S4.S447 


0. 


0.1138 


-0.1138 


-151.2607 


0. 


1.0000 


0. 


0. 



GUT ■ 

0. 
-0.7075 
2.4216 

0. 

•**** SBGOLiTDft QESXOI ***** 

COST FOKTZDI SEICBTHB FACTUM 01 STATES AID CORBOL : 



0. 

4.6700 



ADXAG 
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3C.0 0. 0. 0. 

5DXA0 m 

1.0 

OPTIMAL UTEfflAL HEME AID STATE FZEDBACI GUIS 
CERE m 

•6.4772 
CLCT 

-47.6924 -6.8830 -228.2964 
BECFOUBS 

-1.2208 - 1.00061 
-1.2208 ♦ l.OOOCi 
-7.3891 ♦ 0.7295i 
-7.3891 - 0.72961 

ESTIMATOR DE5ICI 



PROCESS AID KEASUREHEir VISE SP2CTEAL 2EBSXTXES 



QLCT 



1.0 



BLCT 



O.OSOO 0. 

°- 0.1000 



XPEKFI. ZEROS FB0 PfiDCESS ROSE HFUT TO TACHDffiTEE 



ft ACCELEBDHEIEE OUTPUTS 
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TJCHZEBCS 

2.8309 
-2.8309 



-0.0000 ♦ 9.44571 
-0.0000 - 9.44671 
0.0000 ♦ 0. i 

PUITPOLES • 

-7.4486 
7.3232 
-0.0216 



CORPBOKnE ZEROS FOE S7WOIC ttOTLOOT OF ESTDIATOE 



192.0571 
-192.0571 
3.0991 
2.5927 
-3.0991 
-2.5927 



miBML BTDUim CUBS AID ESTIMATOR POLES : 
ESTGADB - 

-€.3092 0.6621 
15.7493 -1.0452 
-0.7363 0.1181 
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FRAME F 




Pendulum position 
if no control torque 
had been applied 



direction of 



frame acceleration 113 




for forward pitch :6> 0 
pendulum control torque 
Q (LGT) > o ; 



for forward acceleration 
of the franv»? 

( x^q* rR3 e) > o 

pendulum control torque 
Q (LGT) < 0 



tte ITT 'Tf 1 ^ ^ SerV ° aCCel — ter is proportional to 
total pendulum control torque: C p (LGT)- k a [ ge - (r q + r J) ] 



Fignre N.l: Longitwiinal Acceterometer Measurement 
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Appendix O 



Discrete Time Longitudinal 
Control System 

O.l Sampling rate selection 

The continue time compensator designed in the previous section mnat hedli _ 
creuzed for implementation a. a digital control system « the on-bc* compel 
of the unicyde. The selection of the best sample rate for the digital computer is 
a -^promise among many factors. Fust of all, the sample period mnat long 
enough to aDow all the control calculations for the longitudinal and lateral control 
system, to be completed. The sampling theorem state, that in order to recon- 
struct an unknown hand-limited continuous signal from samples of that signal, the 
sample late f, most be at least twice as high as the highest frequency stained 
™ V,ia0n We can consider the input signal to a digital controller 

to have a spectxal content up to the system's dosed loop bandwidth, f c . The 
longitudinal dosed loop system has a bandwidth of less than 0.5 Hz, so that the 
fundamental lower bound on the sampling frequency should be approximately 1 
Hz. In order to have acceptable tracking effectiveness of the reference input com- 
mand, good regulation during random disturbance input, and rejection of errors 
due to measurement noise, it is customary to use sampling rates of at least an 
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order of magnitude grater than this val ne (Refer to chapter 10 in [RauldmD. 
Experiment, with the microprocessor mounted on the unicyde haw shown that 
all the control system calculations can le done in less than 25 milliseconds. A 
sampling frequency of 40 Ex is therefore chosen, Le. 



T mmv u = 0.025 sec (0.1) 
We can associate a pure transportation delay of approximately one half cf a sss- 
ple period with the sample and zeroorder-hold (ZOH) device which converts the 
analog plant signals for input tc the microprocessor. 

Gzoni*) = e'f 

where a it the Laplace operator. 



The phase shift added to the openkop system due to the sampling process is 
calculated, because it can decrease the phase margin of stability significantly if 
the sample rate is too low. At the closed loop bandwidth (f B W) of approximately 
0.5 Hz, the phase lag due to the ZOH is 



This small phase lag should not have a serious effect on the system stability. 

0.2 Equivalent discrete LQG compensator 
0.2.1 Discrete time regulator 

A discrete time regulator with integral error feedback of the wheel speed, ft, can be 
designed by first discretizing the continuous time plant matrices of equation 1U0, 
using the sample time T, defined in the previous section; 



<*+D fi .*«][«(*>] [r« 
J [o * J | j f r 



«(*) + 



0 



r(fc) (0.2) 
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where [*(*)= %k) Q(k) $(k)f 
and r(k) is the reference input. 
The measured outputs 2xe 



9m(k) = Mz(kj+Nv(k) 



(0.3) 



Simpify equation 0.2 to: 



where Xl (k) = [<k) m , (Jfc)jr (Q 4) 

The continuous time cost function of equation N.29 can also be converted to 
an equivalent digital performance index as described in Appendix D of reference 
(Bryson 1). 



Given the continuous time performance index 

/ = ^(~(^/+« T ifc.)* ( o.5, 

the CTRL-C program Tgtdlqg.ctr' uses the algorithm by Van Loan [Van Loanjto 
calcnlate the weighting matrices in the discrete time performance index below, 
which will produce the discrete closed loop system ^h«e performance is the same' 
as that of the original continuous time system. 



fesO 



AD ND 
ND T BD 



*/(*) 
«(*) 



(0.6) 



Tfc* QZ algorithm in CTRL-C doe. not currently support the cross terms ND in 
the cost function above, but if we transform the system in equation 0.4 to 

*K*+!) = {*t~?iBD-K ND T )xm + Vt ^ k) (07) 

«(*) = n(k) + BD- i .ND T z l ik) = -Cz l (k) (0.8) 
the cost function of equation 0.6 becomes 



J D = £ - ND • BD' 1 • ND T )zt(k) + « T (k)BD3(k) 



(OS) 
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We can now qm the standard QZ algorithm to solve for the regulator feedback 
gains C of the system of equation 0.7 and modified cost reaction of equation 03. 
The desired feedback gain for the regulator u(k) = -CtTj(k) is then from equa- 
tion OA 

C i = C-BD- l ND T (O.io) 

A print-out of the results of this program is shown in section 0.2.3. The optimal 
feedback control law, with all states available, is: 

(0.11) 

A pole mapping of the continuous time regulator poles of section N.6.1 by means 
of the z-transformation, z = e* r , confirms the locations of the equivalent discrete 
time design's regulator poles as indicated in the printout listed in section 0.2-3. 
The step response of the discrete plant shown in Figure 0.2 is also the same as 
the continuous system step response of Figure N.7. 

0.2.2 Discrete time estimator 

A discrete equivalent of the continuous time estimator of section N.6 is designed to 
estimate the plant states from the two sampled data measurements of the tachome- 
ter and accelerometer signals. 

The spectral densities of the process and measurement noise inputs, Q and R. of 
the continuous time system must be converted to covariance matrices QD and RD 
for the digital estimator. Appendix D of reference [Bryson 1] shows that QD can 
be found using duality of the regulator performance index design conversion. 



*(*) = -C € c(;fc)-C 



m 

a(Jfc) 

m 



The measurement noise input v^t) to the discrete time estimator is a sample of 
the random white noise input v{t) of equation N.49. For the discrete time system 
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we **i rather model noise with a variance V and a torrelatioa 

Ume T e that i. to ^ ^ ^ rf ^ 

could be generated by the first order system 

+ -«0 (0.12) 
where n(t) is a white noise process with spectral density 

R = 2VT < (0.13) 

If we discrete the shaping filter of equation 0.12 with the sample period of T 
w* hxv* ** 

Mk + l) = e-%t d (k) + n 4 (k) (0u . 
and the variance of the purely random sequence n^k) is 

»-v[l..-*J (0.15) 
as derived in reference [Bryson lj. 

If V is eliminated between equations 0.13 and 0.15, 

r ~.i 

l-c-TTI 

271 * (0.16) 
The element, of the RD matrix were therefore chosen proportional to the element, 
of the gtven spectral density matrix, R, as a function of the correlation time T e 
and the sapling period T.. The choice of values for £ and T. was motivated in 
sections N.5 and O.l respectively. 

A print-ont of the discrete time matrices is given in the next section. It show, the 
optunal closed loop estimator eigenvalues and the Kalman filter gains, L, where 

i(*+U = *x(i)+r« W (017) 

i{k) = i(*) + I[y m (*)-Afx(*)-Artt(Jfc)] (0.18) 
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Equation 0.17 is known** the lime update rf tic KaW filter adequation 0.18 
is called th iMMil apdate. Sebrtitution of 0.17 into 0.18 give, the pre- 
diction estimator equation 

*(* + 1) = ♦[/- LMftk) + *£,„(*) + (T - *£JV><*) (0.19) 

The optimal prediction estimator poie. are theretbre given by the eigenvalue, of 
*[I-LM]. 



The integral error state which is.hownas part of the plant system of equatiou.0.4 
is actually part of the compensator. 
Therefore let 



e(k + 1) = e(k) + B t £(k) + r,u(k) + IV(Jfc) 



where 



lf« = */(l,2:4) 

The regulator feedback law is 

«(*) = -C e e(*)-C*(Jfc) 



(020) 

(021) 
(022) 

(023) 



Equation. 0.19, 020 and 0.23 can now be combined to form the compensator 
system matrices 





i-r e c e s e -r t c 

(iLN - T)C t *{/ - LM] + [UN - r]C 



e(fc) 

x(fc) 



y™(*) + 



0 



r(k) 



e(k) 
x(k) 



(024) 



(025) 



A Mock diagram of the discrete time closed loop system is shown in Figure 0.1. 
The closed loop system matrices are obtained by nsLig the plant equation x(Jfc+ 1) 
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- *x(*) + r«(*) and eliniinating ^ k y Md y ^ (jfc) ^ ^ 
compensator equations: 



SYSTEM 
equation and the 



e(*+l) I = 
[*(*+!) j 



* -rc« -rc 

*lm -rc e *[/-£Af]_rcJ 
o 

r, Hk) 



z(k) 

I «(*)-. 



(0.26) 



If the initial estimate of the state vector i, the same a. the initial plant state 
vector, the step response of this system win be the same as the r«ponse for 
the fun state feedback system of Figure 0.2. An example of the response when 
'(0) = 0.1 rad, but the initial erfimate *?) = 0 rad i, shown in figure 0.3. It 
can be seen that large excursions of the plant states occur initially because the 
control commands are «med based on the wrong estimate, of the plant state.. 
As rhe LQG estimator improve, it. estimate, of the plant state., the wheel speed 
approache. the commanded value of ft. = 0 and the frame is held vertical (9 = 0 
rad). 



0.2-3 Optimal regulator and estimator 
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SYSTEM 



TOADT STATE 
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0.3 Modal decomposition of LQG compensator 

The number of numerical calculations can be significantly reduced if the compen- 
sator of equations 0.24 and 0.25 is transformed to modal form. Implementing 
the compensator in this form is also numerically less sensitive. 



Rewrite the compensator as 



r c (*+l) = Acxtf) 



«<*) = Ccx e (jfc) + 0 e 



l r(&) 

[ *»(' 
Ik*) 



where 



0 -1 
*L 0 



= [B, B r \ 



Let the modal coordinate state vector be £(*), then 

x(t) = r«fc)=>f(Jfc) = r- I z(fc) 



(0.27) 
(0-28) 



(0.29) 



(OJO) 



where r is a non-singular transformation nitriz, whose columns are the right 
eigenvector dA* above. 



Wfth thi » transformation applied tc equations 0.27 and 0.28 the modal form of 
the compensator becomes 



+ = A m ((k) + B„ 
«(*) = C m ftk) + D n 



Vm(Jfc) 
r(k) 

*»(*) 
rik) 



(0.31) 
(0.32) 
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whore 



c m = c e r 

D m = D e 



(0.33) 



^isadiagonaJ matrix if ^ has oo repeated The value, of the 

c^p^sater aatrices in modal form are listed at the end of Section 0.23. 
Notice that since D m = 0, 

«(*) = c nW (0J4) 

The closed loop system with the coapen»tor in modal form then become, 



f 

[«*+!) 



*(*+!) 



B r Af A, + B,XC m 



0 



*W (0.35) 



The compensator is implemented in the on-board microprocessor of the uuicyde, 
t he FORTH [Mach2] progxamm^ ^ Since caJcnlation of the control 
T—— * » W"«^y25%ofthe«mp t n g period, apredictiou 
compensator is implemented to calculate the correct control command at the next 
sampling instant, based on measurement, at the present sampling instant. The 
algorithm is summarized below: 

- At the sampling instant, issue the control torque command u(Jfc) that 
***** doing the previous sampling period, to the wheel drive motor. 

- Head th* sensors to obtain y m (k) and r(k). 

- Update the compensator states by using equation OJl: 



*»(*) 

r(k) 
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- Calculate the control couunaad for the next sampling instant, «(* + i) f by 
using equation 0 32: 

*(k+l) = C m ((k+l) 

- Wait for the next sampling instant and then repeat the loop. 

In order to start the kmgitodinal system with minimum transients in the control 
torqne the beat estimate for the initial values of the estimator states should be 
used. The unicyde is first brought up tc tie nominal speed flo by using feedback 
of the tachometer signal only. No attempt is made to stabilize the unicyde during 
this phase, and the operator uoii* u«s uaicyue «a v«ii«l -» pWuie during the 
speed-up phase. The plant model simplifies to a first order system 

[I? + (™w + m F 4- m T )r^r]Sl + f w Q s Q w (0.36) 

with the coeffidents as defined in Appendix M. A PED compensator with integral 
error feedback control was designed, with the transfer function 



q w (z) = ir.i-ipw*) - «(*)] 



(0-17) 



K t _ 16 and * = 0.9 gives a good time response of the longitudinal system dur- 
ing the phase where iae whed speed is accelerated to the nominal operating speed. 

The initial values for the plant and estimator states when the nominal operating 
speed is reached, can be calculated from equation 0.35 by noticing that in this 
steadystate condition 



*0 




x(*+l) 




z{k) 


> 











and r(*) = flo 



(0.38) 



so that 



10 




J - 




> 






B V M A m + 3 v NC n 







0 






Br 



(0.39) 
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Whe=C3 W!Sa<i& * — to the compensator state, in the 

on-board computer. 



A listing of the FORTH code that indent, the longitudinal control algorithm 
is shown in Appendix P. 



0.4 Pcle-zero compensator with accelerometer sen- 
sor 

Control systems designed by succerive loop closure tecWue. not only produce 
compensator, of low order, but usually have better robustn«s to plant variation, 
than LQG designs. 

We w21 w the same sampling period as for the LQG derign. The CTH..-C pro- 
gram Tgtdtach' is used to design the controller and a listing of the program is 
shown in the uert section. The on board microproc«or on the unkvde take, 
a finite time, 2^,, to complete afl the calculation, for the compensator This 
delay is incorporated in the plant model a, a pure transportation lag by means 
of a orst order Pade approximation before the plant is discretized. Section N*2 
describe, how this add. an additional state to the plant state space modeL 

The first loop closure in the compensator design is a proportional feedback of the 
tachometer signaL Figure 0.4 shows the rootiocus of the tachometer feedback loop 
asafuactfaDofthefeedbackgainJir,. We would like to select ^ as large as possi- 
ble since thi, would decrease the nonlinear effect, of stiction, Coulomb friction and 
faction variation, due to the eccentricities in the gear, of the wheel drive system. 
Figure 0.4 also show, that there is an upper limit on the magnitude of K t be- 
cause the damping in the tachometer feedback loop can become unacceptably low. 
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The accderoaeter feedback loop * cW W Waa oa^bkpdei. p^t, 
a compensation pole is placed close to it outside of the unit circle. A compensation 

JW^lVh-ll.l.lk.drt'fc,^^ TheWboundon 
K. is determined by the gain for which the low frequency locus enter, the unit 

the subflity region. Withont careful compear de«ga it can happen that the 
damprag on the low frequency closed bop pole. ■ not yet efficient before th- 
d^ pi=g on the high frrqnacy pole, become unacceptably low. The final d«iga 
of the compen»tor was obtained through "il^ ^ 1^,. rf t fc,«fc»rf 
loop step response. 



In order to simulate the time respond of the dowd loop sy«em the comperuator 
was modeled a. shown in the block diagram of Figure 0.6. The block diagram 
.hows a first order compensator in the tachometer loop for greater generality, but 
•t can be changed to a proportional feedback controller by letting p, = z, = 0 



The plant equations are 



9m(k) = Mz(k) + N*(k) 

whm *>W » tachometer, *<*), and acceleromeier, *(*). 

The compensator matrices are 



(O.40) 
(0.41) 



*.(*+l) 
*.(*+!) 



0 Pa 







+ 


-l -jr. 




*(*) 




*.(*) 




0 -1 




».(*) 



KrK. 



T{k) 



(0.42) 
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+ [K t K a K T \r{k) 



x(k + 1) = ^i(Jfc) + ^(i) + j^j 

+ [• 



(0.43) 



*.(*) 



[ *(*) J 



or 



s<*) = Cd(k) + D^k)* 5 e r(*) 



(0.44) 



(0.45) 



The value ot n r is selected so that the transfer function from r(k) to* a (fc) i» unity 



is. steady state: 



so that if, most be 



lim^ = lim^.i^il 
«-» r(t) *-i 

i-p. 



K, - — 1- * 
if. 



(0.46) 



(0.47) 



We can now combine the plant equations 0.40 and 0.41 with the compensator 
equations 0.43 and 0.45 to obtain the closed loop system equations: 



*(*+l) 
r(fc+l) 



where 



* + rDcJM T(C e + DcJNC t ) 
BcJM A c + B c JNC e 

nsc+DcJNSc) 1 

Bc + BcJffSc 



z(k) 
*(*) 



r(k) 



(0.48) 



(0.49) 



Figure 0.7 sh«-s a step response for a 1 r/s command in wheel speed. Figure 0.3 
shows the loop gain and phase versus frequency of the longitudinal system 
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CU.1 Successive loop closure compensator j>arameters 
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ICCtOQHQ • 

0.93S1 ♦ 0.162S1 
0.9782 ♦ 0.02811 
0.97B2 - 0.02811 
0.93S1 - 0.162S1 
0.0337 - 0.00001 



0.5 Pole-zero compensator with vertical sensor 

We will TO the same successive loop closure design approach a. ui secuoJ u.4, 
but this time assume that we have a sensor thai measures the pitch angle, 8 . The 
first bop closure in the compensator is feedback of the tachometer signal, with an 
integral error compensator consisting ofapokat* = landazeroat*,= 0.75. 
Figure 03 shows the root locus of the tachometer feedback loop as a function of 
the loop gain K t . The print out of the CTRL-C program that was used to design 
the comp e n sato r is shown in section 0.5.1. 

The pitch angle feedback loop is dosed next, with a first order compensator where 
the pole is ai z = 1.03 and the «ro at * = 0.95. The root locus of the outer 
loop is shown in Figure O.10. Notice that the high frequency root locus enters 
the unstable region of the i-plane far high loop gains \K,\ > 150. Thisisadg- 
nifieant improvement over the situation with the accekrometer sensor where the 
high frequency root locus branches were strongly attracted by the pair of complex 
zeros just outside the unit circle. The enlargement of the low frequency root locus 
branch near* = 1 (Figure 0.11) shows that the loop gain if, was selected so that 
the dosed loop poles were weQ damped. 



The simulation of Figure 0.12 shows a time response of the dosed loop system to 
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a unit step command in wheel speed. 

0.5.1 Pitch feedback compensator calculation 
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Figure 0.1: Blockdiagram of a Discrete Time LQG Compensator with Integral Error 
Feedback 



ts 

I 



CO 



306 APPENDIX 0. DISCRETE TIME lOlWITDDATAi CONTROL SYSTEM 

DISCRETE SYSTEM RESP0NSE WITH FULL STATE F. B. 
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Figure 0.5: Rootlocns of the Accefcrometer Loop with » Furt Onter Pole-zero Com- 
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APPENDIX 0. DISCRETE TIME LONGttTJDINAL CONTROL SYSTEM 
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Figure 0.6: Blockdiagram of the Discrete Longxtiidixud Successive Loop 
Closure Control System 
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Figure 03: W Loow of the Discrete Time Lcngitndinal Tachometer Loop with 
Integral Error Feedback of the Tachometer Measurement 
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Figure 0.10: Root Locus of the Pitch Angle Feedback Loop with a Tost Orier Com- 
pensator 
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Figure 0.11: Enlargement of the Pitch Loop Root Locus in the Vicinity of z = 1 
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Appendix P 



FORTH Codes for 
Longitudinal Control 

P.l Successive Loop Closure and Accelerometer 

The FOSTH [Mad*] code which implement, the compensator designed by suc- 
cessive loop closure (section 0.4) with at accelerometer sensor, is listed below. 
Daring the first 200 sample periods, the control algorithm closes the tachometer 
feedback loop only, to bring the wheel speed up to the nominal speed, fl* The 
operator tries to hold the nnicyde robot as vertical as possible durinr this stage. 
Thereafter the algorithm closes the accefcrometer feedback loop to start the bal- 
annng process, 

Afl the compensator calculations are performed in fixed point arithmetic with a 
64-bit word length, in order to minimize the computational delay. The tachometer 
measurement and control torque signals are scaled up by a factor of 10 3 and the 
accelerometer signal is scaled up by a factor of 10« in the computations in order 
to maintain good numerical accuracy. 
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APPENDIX P. FORTH CODES FOR LONGITUDINAL CONTROL 

The program listing » documented with comments to explain the logic of the 
ajgorithm. 



— Loogitodiaal Control Tarakoy Prograa 



( Progm. to f t ucjcS/lgtdtoch coatrollox : Too^l. . » mUti-toi* ) 
( Tight uch f oodfaock loop doMd first ) 

o tehift • 

UUD/TA*zr ( Turn ocholag off daring do.nlo.dii* ) 



SEX 



800000 COBTiiT UTDlClflOt ( Sot LIT DoC'o oddrooo ) 
800010 C0BT11T LGTDACADOft ( Sot LCT DoC'o oddrooo ) 
800020 CMSTAIT UffiKUO* ( Sot Uff Eocodor'o oAiroo. ) 
800030 CDETA1T LCTEKoflML C Sot LCT EacxJor'o 



) 

500080 <**MT BCC0DHO0 ( iUdio H^itox Iatorfoc > 

8ooo82 amiMMt wxaanax < countox idmM ) 

800084 CSBH1T tCCDIHT EB 2 

800085 CDBtilT BCC0I1B0UCBD ( *nmfx Controls oddrooo ) 
800090 C0BTUT KlBtGGE. ( Ttiggor oddrooo to BESET IBQ flip-flop ) 
68 CQBTiffT LEIE12JOQ ( Lof«12 iatoroctor Jddrooo ) 



800000 
8000EO 
8000FO 



COBTUT SiEADCR 
COBTiBT HDXIDOR 
COlSTAoT ADCADOa 



( Sooplo ft bid's oddrooo ) 

( Boltiploxox'o oddrooo ) 

C inoloff~to~Dititol Coaiortor'o oddrooo ) 



P.L SUCCESSIVE LOOP CLOSURE AND ACCELEROACETER 
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0 COKSTJUrr UTACOUX ( mXCodo for Lateral ,Acceler©neter ) 

1 C0BTA1T LffTlcanX ( WJXCoaa far fengltadinal iccalaroawtex ) 

8100OF COSSTJVT KTCctrlim ( Set ImI Tin* Clock'* Cntrlesrd iddrtM ) 
810008 C0BTA1T UCcounterl ( S«t ImI TIm docx'» Coaster 1 addraea } 
0 OTBTAIT CHADZLO ( Teminel ua mabtr ) 

DEC DIAL 

IS C0WTA1T ITTIt ( Tach loop gain ) 
-20 C015TAVT XJSTU ( ice loop fain ) 

62500 COT3TAIT LffTXr ( Ir*lC-4 : T*t*tw it*?!* jii- > 

3000 CO 1ST AIT OnegaO ( lOOOoooadnal wheel apaed ) 

500 CO 1ST AFT STOBELength ( v *t «atriea to bo Mf«d in STORE array* ) 

12S0 COBTAIT HTCperiodi ( Real Tiner Clock Tisar 1 initial count ) 
< for 40 fax MBgliag *req > 

VARIABLE LATPW ( Lateral Radio Pttlae Width ) 
VARIABLE LC7T7WI ( Lonflt. Radio Polno Width ) 

VARIABLE TICOATE ( Boozy location to em tier count ) 

VARIABLE LgtfiaeCouBt ( Counter for ahoel op —d o to avera«a ) 
VARIABLE SpoodDjpCooBt C countar far tine in ap ae d up aode ) 

TtfXAfiZ SUflPCaacS ( Coimta nnnbera atored in STORE arraya ) 

VARIABLE LGTCad ( LOT accalaxation coamand «10o6 ) 

VARIABLE LCTXak ( LGT accelaxoeeter conpaneator •tata*10a6 > 



VARIABLE 
VARIABLE 
TABXABLE 



Tacc6 ( Accalaronatar aeaaurenant • 10*6 ) 
Q*3 C Wheel Torqua •10e3 ) 
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40c 1600 B£cs<amn> suite 

400 1000 TESXOiL BOSS 

< ••**** ten. hub cloci Rmaaimno hobos 

C0C2 XTCcet ( 3«ta arc Covmw ; ■ ■■pi t ran J 
«TE.L 00. -(16) ( wm content* of 00 on itnck-) 



WE-B ft74.HTCctrllD0a ( Rewrite. Catrlwwxd lor cotmter 1 ) 

HDfE.e* *8TCperlo41,D0 

WVE-B DO.BICconnterl ( Send low byte ) 

LSR.If ta.oo 

mnfc.B DO.RTCcoiHrterl ( Send hie* byte ).,-.. | 

i* ' - t;. 

CA6)*.D0 ( Bewtore. uridine! rlm^t DO ) 
nTS C Betnxn to snhrootin. ) 



EO-C0DE 

Ca . ( Loaded SICMt ) 



UTS 

FP32 

nmmz stun sraanjm^thz. 2. ulloi; c n** notary ) 

FV1HUBU STUB SIDBELtngtn 2e 2* ViLLflt ( «rmyw I. 2. 3 ) 

FVlKUHtK STQIO STDHELengtb 2* 2* ViLLOT; 



BOLL TDK CLOCI 



SEiOOG B0BDS 



CODE ZEBOtboHCD C - > C Zero. th. tic* con*** r^t^ ) 

CIB.L CIS) 

RT3 

□TO-COEE 



P.L SUCCESSIVE LOOP CLOSURE AND ACCELE&OMETER 



CI .( Loadod ZOOtlMTICKS ) 

am tIClStoSTKt C - * ) < Ua* tick couat « top of atack ) 
CAS).-<15) ( Curratrt tick co«fr walu. onto .tack ) 

ca .< Loadod TICOtoSTACX ) 



i TtimuMuu coram. ST5TM alooiithi uotm mmammmi 

: S 2048 UTOACAM* ¥! 2048 LOTDACAS08 V! ; < Quick motor, top) 

: TWC3ATDU1E (a - a ) ( chock ^ DAC coaaau* Saturatua ) 

OT 4096 > C chock for poaitiia aataratioa ) 

XT DWP 4095 ( dri - lar«o t^uo 8 oqpply M t. Taloa ) 

OSE OOP 0 < ( chock for zMgatiT* aaroratiaa ) 

I* 080? 0 ( drop largo raloo t .«ppi y Mt . t^qo ) 

TBEI THHT ; ( othorviao koop —m li | DAC valno ) 

: VUIT48 < a - ) < 3/V V AIT for o»30 UrmtrnwH ) 
0 DO LOOP ; 

: CHBCnSAfftt ( - f ) ( Chock if m^I. tiao hu .xpir^l ) 

TlCDtoSTACI ( got tick couat ) 

0 > ; ( o«ts troo fl**; if dt >- Taaaplo ) 

CR .( LoaoM C8ECXTSAJ0U ) 



4CTITATE ( activato Backgnrmd task ) 
S ( Stop all BDtoxv ) 



0 LCTCad ! ( Initial 1ST rafaroaca input aaauaad - 0 ) 
0 UJtak ! ( Ialtlalixo acc. coop, atata to zaro ) 



0 SpuodZfeCouat ! ( Initialixa SPEDLtXP couatar ) 



0 STOKECouat ! ( Tirf*^!^ si PHE i couatar ) 
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NCaat ( Sat Baal Tla. clock to Tsaapla ) 
BEGU 

CHECnSAHPt£ ( dock if saspla tin has srpirsd ) 

IP ZESOthonOB ( If THE ftasot coobUt to zaro . . . ) 



** BE10 LOT TACHOHETEB AID ACCEtQOHETEH « 



LCTUClDDft Vt ( road binary Tales fro. «nc. intarf aca ) 
( Bt <- top of stack ) 

ICTACOfJX OTADOt V! < switch IfJX to UJT accalaromatar > 

1 StflADCR V! ( saapl* .i^x > 

I KUT41 ( .alt 30 sicrosac f or Ssfl to sattls ) 

0 SkfiUXa ¥! ( pUco Skfl dsrica ia BOLD aods ) 



1 ADClWm V! ( Starts A to 0 Cosvartar ) 

1 VAIT4J ( bur a littlo tins for ADC to f inish ) 



ADCADOR M ( raada binary Tain, fxoa A/D > 
( Bt Ba <- top of stack ) 

409S - 4884 . 10003660 ♦ ( coarart to a roIta^lO^ ) 
-31676 100C00 •/ ( seals by LGTACCOAH -> acc~10*6 ) 
( Bt Ta6 <- top of stack ) 

D0P TaccS ! ( am copy of accal to prist Utar ) 
( Bt Tafi <- top of stack ) 



Spa«flJpComt f 200 > ( chack if spssd-«p phasa 



is orar ) 



C ************** I0CEl£3QHntt LOOP 0QHPEB5ATXQI 



IP ( if so . add accalaroaatar coapaasatioa ) 



LCTCad f ( g«t accalarstiss tonsand ) 

IOTr • ( salt, by Kr*10s6 -> 10*6 « accal. end ) 



Rl. SUCCESSIVE LOOP CLOSURE AND ACCELEROMETER 



SUP - ( rabtxoct T«6 to jot occ. oxrorolOo* ) 
» ( mm a cop? for comMtor npdsto ) 
( Bt £06 Eo6 <- top of ttad ) 

LCTt»k • ( got LOT icoltniHur ec^mtor ototo ) 
( Bt Eo6 ZkS Za5<* top of stac* ) 

9 100 •/ ♦ ( ttltlply by po-x* tad odd to occ. error ) 

UOTa 1000 •/ ( «lt. by LOT*: di». by 1000 -> 1000*do ) 
( Bt E*6 daJ <- top of stock ) 

EZSZ C dnriog spood-op do toch loop coop, only ) 

DB0P ( drop -To* from top of stock ) 
0 0 ( nppl y xoroo i ox E*6 «ad do3 ) 
< Bt 0 0 <- top of stock ) 

( • Ticawrm loop cohpobatim 

THE! ( do tocboootor cooponsotioa loop ) 

OaogoO ♦ C vbool spood ♦ do3 ) 
C Bt Eftfi sua <- top of stock ) 

B0T ( got binary toIoo of too tocb ) 
C Eo6 m Bt <- top of stock ) 

MP < copy to dotoadao sijn lour ) 
1091976 SUP / ( got lOOOoobs(ytoca) ) 

SWP 2 BD 0«(«t si«n of too obool spood : ) 
IP IB6ATE THE! ( 0 -> oroa t -> nog. spood ) 
( Eo6 soa Tt3 <- top of stock ) 

HF Ttocb3 ! ( sots o copy to prist Utox ) 
< Eo6 in Tt3 <- top of stock ) 
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( got velccity «rxorol0«3 ) 
- ( Ea£ Et3 <- top of stock ) 

LCTtt • ( salt tach loop gaia ) 
( -> 1000»«hMl torqqo ) 
( Ea6 Qw3 <- top of a tick ) 



OTP Q*3 ! ( Mfca a copy to prist Uur ) 
C E*6 Q»3<- top of stock ) 

15120 ♦ 2048 15120 ./ ( got huury co»oad for DAC ) 

?DACSATURATE ( chock if DAC sstoxstos ) 

ICTOACAMt V! C output ronpio coaod ) 
( Es*<- top of stack ) 



v CCKPEBATCt STATE UPDATE 

LGTxsk t ( got LCT occols 



liar stoto ) 
^ E=5 1=5 <- tap of stock ) 



t*T*k ! < odd to to. sec. orror sad xhst0r>O ) 



105 100 •/ ( .duply by LCTfcs > 
♦ ICTrsk ! < odd to ths sec. or 
C otsck oapty <- top „f ;tock ) 

♦! ( 



1 TDK HISTORY ixmmu 

STUREOwmt t STUEELsagta < 

IF 

Q«3 i I>P 1000. ft < Scslo to oor»sl units ) 

STDHE1 STOIECoont • 2* 2* ♦ F! ( soltiply iodsx by 4 to ) 

C storo 64 bit $ ) 



Ttach3 t I>F 1000. F/ ( Scslo to nozBsl omts ) 



P L SUCCESSIVE LOOP CLOSURE AND ACCELEROMETER 



TtOMEl STDKZCotmt • 2* > ♦ fi 



Tac<* « I>F 1000000. F/ ( Seal* to mmml tmlts ) 
STDBE3 STOttConat f 2o 2o ♦ ?! 



1 STOtBCowt ♦« ( Indent tho cwmtox of t otoxod ) 



THEI PA05E ( Go do tracts*! talk ) 



( Bonrao or rami. * uamam tasb rot ramr ajvlicatim 



( Baild nd Activato 
SUVE BOIID 
SU.TE mm ; 

: TKZT ( Chock ifbjiu 



FUSE 

TCEBHDUL 

OOP 

IF 



: PHUT ACTIfAIE ( Activato TotW Utk ) 
-1 HIT-0I31 ( rtttt all disk drlm ) 

bbgxi 
ca 

Proao any koy to too Bl CimttJUlD variable " 
?IET 



326 



APPENDIX R FORTH CODES FOR LONGITUDINAL CONTROL 



CR Qw-C - 
STORELangth 0 DO 

STQSE2 I 2* > ♦ Ft 

I 9 HDD 0- XF .. - ca 

TH25 f. - 

PATS£ LOOP. ]; " CR 

ca TtacW - 

STOKELcogtb 0 DO 

I 2« 2* ♦ Pi 

I 9 !5H> 0- IF ca 

TS1 F. - . 

PIDSE LOOP ]; * ca 

CR .« Tacc-C w 
STDRELcngth 0 00 

3TDBE3 I 2« > ♦ Ft 

I 9 K2D 0* TP CR 

THEM ?. « 

P1E5E LOOP J; - ca 

ca 

ACAU ; 
: START 

CBAlino BOSS TOLD ( Initialix«a Taxaiiial Taa* ) 
BOSS PRUT 

RBZP ; ( Initially— Backgroiairf Taak ) 

TOUETSTOT ( ToapPDjw. iato . TOaHEf application ) 

ca ca ca 

.( ttriu srotact STATIC hmzt and rwx tha coapctax ) 

EOP 



P.2. LQG COMPENSATOR WITH ACCELEROMETER SENSOR 
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P.2 LQG Compensator with Accelerometer Sensor 

The FORTH code which implements the LQG compensator (section 0.2 »sd 0.3) 
is listed below. The algorithm has an initial phase to bring the robot up to the 
nominal wheel speed, fto, where after the LQG compensator is invoked to start 
the balancing process. 



The compensator is implemented in modal form since it requires fewer arithmetic 
operations. Floating point arithmetic is used because of its convenience and the 
fact that the calculation time is not that critical due to tue Action estimator 
that is used. 



( CTO-C progroa : ocjcS/letdlqg.ctr ) 

( — ~ — longitudlMl Control Ttxaktj Ptogrwi 



( DiMcntm LQO co^ooootor wit* Urt^rml 



of 



0 TEHFT ! 

LOUVTAMET ( Tm •choii^ off (taring domioodlag ) 



( • (vue ADDKSSES iWD CtmSUMTS 



800000 CUSTilT UTDAClim ( Sot LAI DAC'» oddrooo ) 
800010 COBTUT LCTDACADD* ( Sot LCT DAC'a mUtooo > 
800020 OTSTUT UTEBCAODft C Sot UT Eocodox'o oddross ) 
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80OO3O COBTUT UnOCUMM, ( 3«t LCT 



800080 cobtut sceoorato ( uau> B*-=i--r istsri ««• ) 

8C0082 COBTUT iCCOQITBU ( Comrtw U wm ) 

800084 cobtut actumm a 

80008* amtm tcconuunn < <w« controiwrd .ddroo, > 
8<w*> cDBTiiT nma ( Tri«« „ ^ ^ , 

«8 COBTUT IBfmjBQ ( U*^2 intoTocUr Jddrmo ) 

80OOD0 COBTUT SttUDU ( S«^l« * bid's Mitrm ) 

8000EO COBTUT IDXlOCt ( Italtiplw. oddr*** ) 

8000PO COBTUT iKUHL ( InUof-to-Oi^tsi G»m««r'. 

0 COBTUT UTlCOm ( IDXCod. for Ut«si J«^«»«« ) 

1 COBTUT LCTACCHH ( MUCodt fop Um«itadin«l icd«r«»t«r ) 

81000F COBTUT BCctrUMa ( !« ImI Tta dock-. Cntrl-ord «Jd»» ) 
81000B C0SSTUT KKomUri ( S«t tel Ti» Clock's Cowtor 1 ^d«« ) 

DBCTJUI. 

TUIUIZ LATFBR ( Utaral Radio Pol*, gj dta ) 
VUU1BLE LCTPW ( Lonjit. Svtio Pol^ Kdta ) 

waiutt unices ( r ,„ Ut«ml Cnrr-tt ) 
TUIUtt LCmCQB ( Co— dsd Lo^tndfcMl Corot ) 

TUZIBIE TXCBME ( Henry location 



to mi* tick count ) 

TUXUtt LgtSfUg ( SFEZD.V / B1LUCE Uf ) 
ItXIimz Lgt&cComt ( Cantor lor • .b^a .p^d. to aTCncs ) 
7UXU1£ spaadDpConnt (Counter tag period in SFSED.V i 



MBIABtt STOHBWt < Co«t, mate, stand in STfflE .my, ) 



LQG COMPENSATOR WTTH ACCELEROMETER SENSOR 



TDciM Ptticm* ( l -> panic ; 



0 -> 



•▼vrjthing QK ) 



0 COBTiVT gw*wft * > 



.4 •»w 1 «ri 0 ui » m run Cioei Tim i •— - w ._ 
■JTE : that do Sadie Initiation cod. BERK fcth «. , 

( Tn«« a i«i,.. aaa lx — rabid nscsnn izaohq wn* wMtM mt n ui„ y 

COOK tlDIOJEdVD ( Booo* Radio locoivtr Cbamls apcm IRQ ) 
mn.L D0.-<16) ( MfM contonU of DO on it^k ) 
HHB.L $24(14). -(16) 
B01E.L 06.(13)* 

06. tt 
•Wtt (16)*, D6 

WTE * B ^1.«COTT1011TO ( Litcbao *U carat* > 

"D?E.B DO.U7?«ll ( tonpanlj > 

WfE.H KCOOrail.00 ( tad. SSB of Comt«rl ) 

LSL.W «8,D0 

WTC.B U7PW.D0 ( Qmt LSB ) 
WJE-W DO.UWB ( 3m ) 

WftH aCCOTTM.OO ( EU^ LSB o* Cowfx2 ) 
WTE.B D0.LB1PWI C Sato tnyortlj ) 

wim accronao.Do c ion* ssb of co««r2 > 

UL.V M.DO 

■JVE.B UZ1FW.00 ( Get LSB ) 

■WE.B t$8F,iCC0m0LTOO ( R*«rit«« Catrlwwd to 1 ) 
WfE.B BO.BCCOTtnu ( Utt LSB ox initial countcrl ) 
BILB ».BCC0OfIttl ( Krito SSB oi initial comttari ) 
Wf^B «!4F.aCC»TB0tsraU) ( Bonritoa Catrlwort to 2 ) 
■WE-B B0.BCC0DREK2 ( fctto ISB ox iidtial conttart ) 
■TO.B B0.BCCOUREB3 ( ttrif BSB ox initial counter? > 
ICTE.B tl.BCniCCEa ( R**at HQ flip-flon on Radio Int ) 
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IOTE.L 06. 0* 

-(13) .06 
tCVLL U6)+,t?4(A4) 

HDVE.L U6j-.~! ( lootoroa ori^Ual mlno of DO ) 
KTE C fUttrn* from oxcaptlon mtiat ) 

OB-CODE 

CE . ( Loaded IACT O gin ) 

: RAKOJUT ( Initial!,. Hodio Control Iat«rt «^ t Int«rwpt ) 

t'J HlDIDJZiO ( Inlci*U» Mio bctinr ... ) 
LEm^IBQ ! C ... Ut^ 2 iato^ctor ) 

SHUT 

10001111 KCOTTWtWID C! C Sot ConUn t,a ) 
01001111 BCCQVTBOLB08D C! ( for Hbdo 0 ) 

1 BCTBISGa C! ( Rooot OQ dip-flop an Radio Iatf ) 

RADIO JUT ( Do radio rocoif«r inucna* initialixntian ) 

C KHE : tot do Udto InitUlixntUa cod. BEFORE Rat* oordo art inclodod ) 

( " Tl n i ii HEAL TDK CL0CI BEFBOOUmZK WBD3 <MM(mu j 

CODE SXCMt ( Sots RT Clock Cotflrtor 1 to M^Ot rata ) 
H0VE.L D0.-<*6) ( htm cantanta of 00 an stack ) 



IOTE.B ^.SJCctriiooa ( awnto. Catrlaord for counter 1 ) 



R2. LQG COMPENSATOR WTTS ACCELEROMETER SENSOR 



WfE.W «RTCp«rlodl.DO 

DO.rrCcoxarUrl ( Soad low bjtm ) 
L3E.» S8.D0 

HHE.B DO.HTCcotxntarl ( Soad high byto ) 

W7E.L U6H.DC ( B«tani original nlm of 00 ) 
KI3 ( Botaxn to •ofarootiao ) 

ao-eose 

CE .( Loodod KTCaot ) 



U30 HATH < inclaoa floating point arocaaoor ionk ) 
FP32 ( sort for 32 bit floating point motors ) 

3. FC0BTA1T 0— g>0 ( loaiaal «ho«l 



19.8 FCOlSTUr UTlOntani ( Latnral lO^Ijma^Itort^. ) 
15.12 PC0BT11T LCTlOlEtorq ( tonal t. lOlguttttoi^ ) 

0.36048 POaSTAJT UTICOUH ( UT lccol'a.tox calihr constant 
-0.3tS7S PCOBTilT ICTACCCAI? ( I£T ict-l'ast-r calihr conatant 
( in aaits of n/m^Z par volt ) 

1. FCOT3TAIT LCTpt ( LCT tach coap polo ) 
0.9 JCMBTAir LCTxt ( LCT taca coap raro ) 
16* fCBBTUT UOTt C LOT taca coap gain ) 

FTaKABU LGTcoaaand ( LOT coaaaad froa radio racalTar ) 
FVUXABLE LGTtaca aa aaai ( LOT tacaoaotar aaat ) 
FTABZiBLE LGTncc_aon» ( LOT accolaroaatcr nu ) 

FTARZAStE UTcoaaand ( LIT coanaad fro. radio rocoiTar ) 
FT ART ABLE UTtacfcuaaaa < LIT tacaoaotar aaao > 
FT4BUBU UT*cc_ao*o ( LIT accalaroaator a— a ) 
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F7AHXABLE thrtadot ( Plant that*.** ) 

TTlZZiBLZ oa«ga ( PUat oa»ga ) 

FTARIABI2 thru ( Pl*ot thtU > 

FTABZABLE ISTYalEzxor ( LOT nlocity arror ) . 



FYAHIAHLE LCTrt* ( LCT tach loop 



co^oMter atata ) 



F7ABHHLE LCtthatl* C aodal co«pasator attt* ) 

niKimZ imihMXTx ( «o<lal comparator atata ) 

F71RUBLE Iin*hat3k ( aodal center .tat* ) 

FVAKUBLE LCT*hat4k ( aodal co«pa»»ator atat, > 



FVAKUttE EtaSiga ( Sl^a oi Tarntabl. apaad ETA ) 

FT1RXABLC 0-s*Si«p ( Sign of WsmI speatl OBU ) 

FTASXABLE LffTamtrolTorq ( Qcoatxpl - Qv of aia*Utloa» ) 

FYARIABLE ICTTorqlTwrng. ( Ararat L«*jit Torqn. - Qfric) 

F7ABIABLE ICTtot^lTozq ( QtoUd . Qfric ♦ Qcantxol ) 

FYARIABLE ChMgaOA^axag. ( Ai*xa«* to-1 Sp-d ) 



F7ABIABLE QaogaOAccn ( Bboal Spaad Accumulator ) 
FVARIABU LOTwrqAcoaa ( tonglt Tamo. Acaaa. ) 

FT ASIABU SSfcELaaftA 4 • TAUffT ( Tia» history ) 

FTAHIABLZ ST0IE2 STUHELangth 4 • TAIXXJT ( array* ) 
F7ABXABLE STWD STDKELaagta 4 • T ALLOT 

400 1000 6*0*0000) SLATE 
400 1000 TOTHAL B033 



( 



radTAL-TQ-AMALOG COl fBgE I HOBOS 



LQG COttPENSATOR WITH ACCELEROMETER SENSOR 
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V4Q 6000 0 DO 10OO 1000 • DtflP LOOP ; C Coaaral parpen 3/V wait loop ) 

: ?TUCSiHailE ( a - a ) ( if DAC cowawaid Sataxatow ) 

KF 4096 > ( chock for pooitiTW aataratisa ) 
IF MOP 4D?5 ( drop largo ^ ft «*ppl 7 Mt . toIuw ) 
ELSE OOP 0 < ( chock for amtin aataxatio* > 
IF OMZP 0 C drop Urg. toIuo .ft .qpply sat. walaa ) 
THEM ; ( othorwiso ko«p co— mood DAC rmloo > 



: L47DAC ( fa - ) ( Cot 12 bit fadaary <-> Qt ) 
UTIOntorq P/ < dirido by 10oI«oox»Itoroao ) 
1. FSVAP F- 2048. F« 

F>I TDACSATOTUTE ( chock if 0 < HUAIT < 4096 ) 
'-ITDAClCCt W! ; ( arito 16 bit lord to ; 

: ICTDiC ( fa - ) ( Cot 12 bit binary <-> Qw > 
FMEGATE ( oo that Qw > 0 caaooo OavaaO > 0 ) 
Umontorti P/ ( divido by lO-IjoarUtorqao ) 
1. FSIfAP F- 2048. F* 

F>I 7DAC3ATOIATE ( chock if 0 < BHAaT < 4096 ) 
LffI1UCAD0 » : C arito 16 bit wont to LCTWC ) 
Ct .( Loodod LCTDAC ) 

: S 0. UTOAC 0. UTTOlC ; ( Qalck may to .top aotora ) 

( m 



ilALOO-TO-OiatTAL C0 HE5S TODS 



r ttmi < a - ) ( S/¥ tAIT for 
0 00 UUP ; 



< a - ) ( switcbos BOX to opocifiod input 
1 HATT4I ; ( aaito 4 adcroooc. for Id to ootUo ) 



: SUPLEkflDLD ( - ) ( SAJfiU aad HJU> coaaaad ) 

lStaifiOKV! ( aaapio aaalog sigaal ) 

7 BUT41 ( waits 28 aicrosoc. for Ska to oottlo ) 
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"! ; < placa StB <fcrico ia HDLD i 



: iD-COTTOOm ( - n ) C Do Analog to Digizl 
t ADCADSft V! ( Starts 1 to 0 Cooiwrtor ) 
9 «AiI,a i wit. jo marmmz. fox ADC to fiaiaa ) 
AKADD8 W : ( Bort. «te ^ m lwlt troa tbo IDC ) 

: atoO.COHESrC.-a, (^.a^^^.^j 
SAKPLEtaOLD 



" ttADUO WORDS — } 



it ) 



4095 



it=5.C=-TSST ( Bo*4. Ixuary «lu. in» */D ) 

- I>F ( CooTorta to nomUag poirt * ) 
0.004884 ?• 

10.003S6 PSVAP F- ( Cot «nalo 8 rolt *g. > 

UW5C F * < ««*r b, «Uhr. coot^t to ^ ... > 
; C rool oecolarotlaa ooos by tho 



HMTACC < - fa ) ( Baada accalaratioa 



4098 

tcTicoBx Atoo.cmnntr < u*. u^rr to. ^ > 

X>F ( Comvti to n**^^ point t ) 
0.004884 fo 

10.00368 FSVJP F- ( Got analog Toltog* > 

LOTiCCCAIi/o (Wtlpiybycolihr. co.tm to gn ... ) 

; ( rool occolorotlao ooo q br tbt wotr^oat ) 

CR .( lo«dod MjCTACC ) 



( **»~«**«*.,„,„ t posrriDi ocqdeb keibhc sdbos 



it ) 



R2 LQG COMPENSATOR WTTH ACCELEROMETER SENSOR 



: BLUER C - fa ) ( Baad TnraTablo Spaad ETA ) 
UTEKlMt W ( M note of clock paloon coantad > 
MP 2 HDD 0* ( 0 «o «iq anabax -> otptlf* apood ) 
IP -X. EuSl^n odd naafaar -> poaitiva apaod ) 

ELS2 1. euSi^i P! 

THEI I>P ( Comrt aacodax raadia* -5o Oon. pout > 
27.2708 F3UP P/ 

EtoSi^n Pf P. ; ( Toratehlo Spaod ETA in rad/ooc ) 

; RLCTEK < - fa ) < Rood Vbool Spoad 0JB0A ) 
WTWUWa W ( Rood mate of clock polooo conatod ) 
mP 2 HDD 0- ( 0 o cT«n aaafaar «> natatlta apood ) 
IP -1. 0a>8>3^ P! ( 1 -> odd oaabar *> poo it It. opood ) 
ELSE 1. OaogaSi«a P! 

THEI t>P ( Coawrt aacodax raadiag to floot. point ) 
10471. 97SS PSVAP F/ 

OoogoSi^a Ft P* ; < Vfaaal Spo«d QKBQA ia rmd/aoc ) 
CI .( Uodod aijOTEBC ) 

( eoj^ TIME CtOCT REASUO VOIDS 

CODE ZEROtkaTICB ( - ) ( Zaroa tha tick cotmtor ragiotox ) 
CLR.L (AS) 

ST3 

EHMOTE 

CI .( Loodad ZODtboHCXS ) 

COOK TICXStoSTACI ( - n ) ( Looda tick coot oa top of atack ) 
OTZ.L OS) .-(16) ( Currant tick coxmtor rrnlao onto atack ) 

tzs 

Ctt .( Loadod TIClStoSTACI ) 

' a— o oo RADIO BECETVEE REA03S VBBXB —mmmmmmmmmm 



: BLATPWI ( * LotR/C ) ( Road Lateral radio cooaaad ) 
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t-UtVK W S164 - ( Baaova biaa ) 

: 8IOT « < * l*«/C ) ( Raad Looglt. radio 
LCTPW W S164 - ( R«, n biM ) 

Ca .( Loadad RLCTPWK ) 



( **—*»——«— coram ststbi ucosithi tens 



TKBtoSTaCX ( g« tie* comt ) 

0 > : ( Ml tmnijll <tt >- TM.pl. ) 

C» .C Uadad CHECTT3AHPLE ) 

conanda. tacbo- ft accalaroaataoca ) 

WTace_*a. P!(« wthl sccUr^t^ T ... uimjllt ^ j 

RUT.CC ( .eetotiw * 



UTacc_aaas P! ' 



yt ) 



< *"* 1— - *tch x»f ) 

LCTtacHjaaa F! ( 3«T*. loogit. 



BliTZK ( 




RLCTPSH ( 

™* 0. ( Taapowy no addition^ 

F+ ( ftddoo^au Am! apa«d ) 
IGTco-and p. ( sw, Longit. cb— d ) 



UTco««d P! ; ( saw. Utaxal rn — „,, , 



P-Z LQG COMPENSATOR WITH ACCELEROMETER SENSOR 



CR . ( Loadad BZAD^SEMStaa ) 

: PUaTtaxisl ( Plant aodsl for .isolation porpoaas ) 

( Fiat calcnlata y(k) - Hx(k) ♦ mi(k) : ) 
oaa» Ft tfcstadot Ft F- UJTtach_ana» P5 ( sixolata* tach aaaa. 

C.OOll omo Pi thatadot T% F- F» 
4.67 tbtu Ft P* P+ 

-.023 UrTtotalToxq PI f» F* ( diract faadthroua* t«rm ) 
OITacc_**aa P! < aiaalatad acc ■■■■in aaiui ) 

< sianlata atata transitions : xtt*l) - PHI*x<k> ♦ Gl»U«n<k> ) 
1.0162 thatadot Fl Fa 
0.0008 oaasn Ft F* P* 
1.3692 tbata Ft F« F* 

-O-017B LOTtotalToxq Ft P» F* 
( thstadot(k*i) raamias on stack ) 

-0.0446 thatadot Ft ?• 
0.9971 o«n Ft P* F+ 

-3.73S7 tbata Ft ?• P* 

0.0607 ICTtotnlTorq Pi fm 
( iana (k»l) rmiM on stack ) 

0.02S1 thatadot PI PW 
0.0 oaaga Ft Fa F+ 
1.0171 thata Ft Fa p* 

-0.0002 LGTtotalTorq Ft P» P«* 
( tbata(k*l) nuia on stack ) 
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tbatadot P! 



( CR thotodot - - Unto** Pf p. ) 
( CR om& - - omm& W f. ) 
< CR that* - - tbmt* Pt p. > 



( clow to dooirod 0a^»0 ) 



t^ciW o 2>f FDQP ( mmbmr of ,™ « } 
0~8»Oiccm fi F3CAP P/ < c^oOtu tnr^ ^ ^ , 
QwsiOAm^ P! ( mtoxwm tbm wmlim ) 



U^Tor^c- Pt FSUP P/ ( c^cuUt. o^. trictioa tonp. ) 
^TTorqATT^. P! ( .toxoo th. t*1o. ) 



0. OmgaOAccia P! ( Cl« 
0. LCTTorqAcc™ P! ( Cloax? 



tbo «caanl*tar ) 
tho tccmlatox ) 
0 I«t£pcComit ! ( Booot. coiErtox ) 



♦« C iaoont comtox ) 

) 



• 3 > C cl-ck if 300 —pi. porio* lu^ 



IF 1 I^tSMUg ! ( Sot fizz ■> BiUKE 



rontia. mxt txm ) 



CR . ( Loodod GmXAwwrmfQwm&O ) 



Pi F* (tag^Aco* P! ( icc^iUt. Spo^ ) 



LQG COMPENSATOR WITH ACCELEROtiEfER SENSOR 



Urrtot*lTorH Ft ( ft** lttm.cwBdrt wtml torqw > 
LCTTortiiOT. Ft F* LffTTox^lccim F! ( koaltt. torqooo ) 

t £«tEncCoimt ♦! ( Jj^nmmU • *W ^ ) 

l«UfccCowt i tOO > < Cbocx if cooogfa rrnli^ MT ^a <^ -tack ) 

THE! 

CE .( Lo«Wd CHEOL3FEED ) 

: SPEEO.DF ( T*cho««tor PB co^aawtion to .pood op .hool to QtogmO > 
OMgaO ( nominal «fc**l spood ) 

aixnnc FTOP Umach_mmaa F! ( mdi «h~l .p-d amd Mra it ) 

t- LffT?*lError F! ( prttctt fmlocity error ) 

UJTfct LffTrt F- LOTxtk Ft F» UTT«l£rx*r Ft F^ LCTIt F* 
FSOP LCIDAC ( tim vhoal torqoo comt thrm^h D/A ) 
LGTtotalTorq F! ( 5«m tt» control conad ) 

UJTNlError Ft LCTpt LGTrtt Ft F* F* 

ld*t* F! ( 1*dat« tach com? ttata f or am Taamplo ) 

CHEO.SPSED ; ( Chock if woool tpood is up to Qm&O ) 

: LCT.C01TB0L ( Calculate, xbm LGT costr output for this aampl* iamtaot ) 

USTferqavartf* Ft 

LffTcor.-rolTarq Ft F* ( Add control tonpw to friction tortpo ) 
FtW UTtotmlToxq F! ( uit tho total torqw ccaaaadod ) 

( Output tonpM command to choal motor ) 
CR .( Loafed LBT.COfTlOL ) 

s ... > 

( fox naxt Tsample ) 



-0.05*4 LCTxhatl* Ft ?• 
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4.6483 LCTUrK,— M Ft ¥• ?♦ 
-0.5677 LCTarr,— «a Fi Fo F+ 
0 .0S21 LCTconskd PS F* p* 

LCTShmm F! C .tor. xltatCk*!) far ^ T-*l. ) 

0.9314 LCTtttUk Ft Fo " 

C.SSC6 ICTUrh,— ■ Fi F« F*> 
-0.0051 LCTacc jm Ft F* F+ 
-0.0008 LGTcawmd F8 F. F* 



U3Tth * t2k F! C .tor, r2h*t(k*i> for ooxt T«^i. ) 

1.0011 LGT*a*t3k Ff ?• 

0.2574 LgTtyh,M — Ft F« ?♦ 
-0.0476 LGTwccmmm ?% ?• ?♦ 
-0.2435 LCTco— ad Fl p» p+ 

*W-t» F! < .toxo x3h.t(W) for o.rt T«*l. ) 

1.2546 LCT*h»t4k F« P* 

-1.2481 tjCTt*clo»a« Ft F* p* 

0.2868 LCTocc^m FI P» F+ 
-0.0448 ICTco»«d F« F» p+ 

*™*t4k P! ( .tor, x4b.t(k*i) for art T W . ) 



1.1031 UTtUtU F« P. ( cUoOato control torqoo ) 
•2.1475 U^Uta Fl F* F* < co-»d for ooxt To^l. > 



-0.030S LCI*bmt3k Ft F» ?♦ 
1.389S LCTtfaat4k F8 p« p* 
LCTcoatrol?^ F! ; ( uft a (*+l) > 



awraoLjsmra < coram, of ut«rmi 



t loagitBdiaal «ot±o& ) 



P«icFla« « o- ( Qmck tho p*nic flag fiat ) 
IF ( If zero -> » panic ) 
LCTcomwm PI F«F 

-.5 F> 0.S F< . 0- ( ^ .. 5 < LGTotic* < 0.S ) 



P± LQG COMPENSATOR WTTS ACCELEROMETER SENSOR 

IF I PnnicFlng ! ( S#t p«Bic n H ) 

ELSE UJT,COTWJL ( Do LOT ojxtan costrol ) 
THEI 



LGfcooomnd Pt LOTlOtttnrq F» LfllMC 

UTcooonnd PI C got Liunl ftodio Co* ir l ) 

LIXXOIltorq Po ( ut Utoxnl Stick control to»uhl. tor** ) 

UTDAC ; ( Ootjwt m * Ctomt CoMad to Ut ■otox ) 

ca .( Lo«m amwtjsTsmi ) 

: STOEti^Hiotory ( Storo. o tin. Wstory of o uri^U > 

STOHEUngta STDHEConnt t > ( cbocn «n itor ^ ? } 

17 



WTtotolTorq N ( got tho to bo .torod ) 

STDHEi STOHECoont • 2« 2* ♦ F! ( ini 1 onut 
( to otoro PI. Pt. t ) 

"ntwd^onon PI ( got tbm tmrUblm to bo .torod > 
STDBE2 STDKECoaxt f 2o 2» ♦ pi 

UTocc^nooo *• C got too raUblo to bo otoxod ) 
3TDJE3 STOttCouat t 2* 2m ♦ Ff 



! ( incronont iodox ) 

; ( incrononts oxro? pointox > 



: WW ACTIVATE ( Acti*nto Background took ) 

S ( Stop oil bo ton ) 

EADDUUT ( IaitioUxo Radio rocoiiox ) 



0. lOTtotalTorq F! ( Initio! LCT control torqoo - 0 > 
Qm&0 LCTcoomoad F! ( Initial VST xmt inpot - 0 ) 
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OnognO LCTtactiaaao F! ( r W 4^,| LCT tachoaa Ur rrti - 6 ) 
0. Lffracc_aaaa F! < Initial ICT *cc*l a*M • 0 ) 

0. LCTTaltaor F! ( Initial LCT Talocity «rrar - 0 ) 

C. tbntadot F» ( Iaxtialira oUnt • ««o. \ 
OattaO oatgi F! 

0. thru F! 

0. tCTxtk F! ( Iaitialira SPOD.tr coap stato ) 

13.3487 LCTkhatlk F! ( Initialir* tha aodal > 
3-4915 LCTrhaUx F! ( coapaaaator otataa ) 
-3S.7439 ZGTxaatSc F! 
15.2396 LCTchat4k F! 

0. aaaaaOaccon F! ( Zaxo anaal tp^d IcoaaOator ) 
0. LCTTorqiccxxn F! ( Zaxo anaal torooa accnanlator ) 

0. LCTTotqlfarag. F! ( Zaxo friction taroao ) 

0.141 UJTcoatrolTorq F! < Stood? cntrl tonptCQ*) ) 

( for OaogaO ) 

0 tgtEncCooat ! ( Coontax for # anaal »poad* to avanaa ) 

0 SpoodtyCoont ! ( Cooatar that dataraiaoa hov long to SPEED.T3P ) 

0 PanicFlas ? ( Start with no panic condition ) 

0 STOHEComn ! ( initialixa SlOHEi array cooatar ) 

0 IctSBOaf ! ( Start with apaad cp algoritha ) 

RTCaat < Sat Baal Tina clock to Taaapla ) 

BBSZI 

CHEOnSAfftE ( Choc* if aaapla tiaa baa axpirad ) 
IF ZSOthaTTClS ( If THE Baaat cntr - 0 ... ) 



LQG COMPENSATOR WITH A CCELEROMETER SENSOR 
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L « t3Ba *« • 0" < 3p-*-r&**lMDcm Flag - ? . . ) 
IF 3PODJ3P ( o -> spMd op AmI ) 
( 1 -> do UlMacm algatitfra ) 

URLGOmOl ( c*lcnUto LOT control tortp* ) 
STOR£ti**fli,tor7 < Stan* * tin hiotorioo ) 



RaDJEKHB (MiUtk swam .^h.^ 



( PUITtaxUX linltto plant *x*l to pwdict tutu ) 
( «M MosnruMsto 4t next uaplt ) 

LCTCo ^°P d » t * < Opdato LCT c«p .t*t« for o«t T» ) 

THE1 THE! PiDSE ( 0o d» terminal twk ) 



OF TEHHnUL ft BiCXOmiD TASO m TTUKET APPUClTmi mmm ) 



: WOP < 8*Ud ad ictiwto Bock|nn»t tuk ) 
SLiTE BUILD 
3U1E UDUC ; 



: TXET ( Chock if Imj mm frprooood ) 
BECXI 

past 
ietomt 

OWTIL ; 

: iCTITATE ( Ictinto TormiaU t«k ) 

•1 HXXHttSK ( root *U diok driivs ) 
BBSX1 

at 
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Ptw My ktytoM, BiCKGBOQID miabk 

ca a*-r - 

STDHELasgth 0 DO 

S7UHE1 I 2« > ♦ ft 

i t no o- if - ca 

THEI P. > . «• 
PASSE LOOP 

ca n*ch-c - 

S7UREUa«th 0 DO 

STDBE2 I 2* 2« ♦ Ft 

I 7 HDD 0- IF CM 



P4DSE LOOP 

STOHELangth 0 DO 

STORES I 2» 2» ♦ Pl 

i 7 no o- if - 

THEI F. # - 
PiDSE LOQp 

: START 

CHARRED) BOSS BUILD ( IaitUlitt. Ttw*! T«k ) 
BOSS PRUT 

VHtP ; C Initial!** B^dfcrow* Tna* ) 



TORHET StIRT ( Tarn txre*r» . 

*r™ r-^«- into a TTJPWICET *yjJic*tioa ) 



CR CR CR 
EOF 



P.3. SUCCESSIVE LOOP CLOSURE AND PITCH SENSOR 

P.3 Successive Loop Closure and Pitch Sensor 
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The FORTE code which implements the compensator designed by successive loop 
closure (section 0.5) with * pitch angle sensor, is listed below. The particular 
print-oat is for balancing the robot at zero speed, bat it can be modified to bal- 
ance Ig^uUL-JI, ~ speed by rn^ng iue CmegaO constant. 

Fixed point arithmetic had been used and wheel speed and control torque was 
scaled up by a factor of 10 3 while the accelerometer was scaled up by 10 6 . 

Comments in the program listing explains the operations in the algorithm. 



( ————— Loagitattaal Cotrtrol Tttrntoy Fro*r»a ' ) 

( Pn*w to toot TKTcingtdtmch coatxollox : Toojoplo - 2S ■illioocoodo > 
( Tljht toe* foodboct loop cloood firot with iatognl error control) 
( Om LOBET pitch agio muot ) 

0 VEKXFT ! 

LQIO/TIBCET ( Ttzrn ochoiag oft during donloAdlsg ) 

C KFDK amESSES AKD C0BTAIT5 o— ooo«oo«ooooo o— ) 



800000 COtSTAJT UTDICIflM ( Sot LIT DIC'o idtoM ) 
800010 C01STAIT LgTDiCiPPSl ( Sot LGT DiC'o tddrooo ) 
800020 COBTiir UlBKUm ( Sot LIT Eocodor'* addroso ) 
800030 C013TAVT UnEBCAOOt ( Sot LST Encodor'o oddroso ) 
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800080 COOT AIT BCCODTOBO ( BacaiT^ laUrtmc. ) 

800082 cmst.it acommsi ( a>tmt«r jdraaa« j 

20SS34 COBTlfT WDCW BtZ U 

800086 ecSSTAIT RCCOilBOUnBD ( Co«t« Co«rol,or«l 

..... SS-IOSS ( Trigg, addraaa to SESET IRQ flip-Hop ) 

68 CfflBTlIT LEvmjBQ ( LmU tetowetor Irfdnaa ) 



800000 coermsiuin c s^, . gold-, .ddr— > 

8OO0EO ajETiTT KJIAK* ( nauplawr-, addtaaa ) 

8000F0 comair actnn ( Aooiog-to-tHgit*! 



0 COmiiT UTACCSSS ( IDXCod. for UUrU tcclaxo^ ) 

1 OOTUT Urricam ( HOXCod. for Laagitodioal Accalaro^ ) 

81000F COBT^ ^rCc^rlaW* ( 5« U*l Cloci-. Controls addr ) 
81000B COBTUT RTteo-tart < St «« Clock-. Cooowr t .ddra„ j 
0 COmiiT CHimu ( Tarmtoal taak nnatm ) 

DECTJUL 



IS CMST11T tCKt ( Toe* loop gain ) 
-40 COISTilT L0TIa ( ice loop gain ) 
0 COBTlfT OMgaO ( ^ ap^i** ) 

320 COETUT STOROaagtli ( data n*** ax»y STOffii 

12S0 C0m.K IlCp.nool ( B-^ TW Clock TW l initio 
( for 40 la aaapliig froq ) 

TlHIiflLE UTPW ( utax.1 g*di. Pula. 9idth ) 

iwmz unrm ( Lmgit. bxtio jmi»o tidy, ) 



V1HUBLE nCDlTH ( Ihaory location to ^ tic* , 

to .vaxago ) 



81BIABIE IgtOicCoaat C Coorto- f or * 



P.3. SUCCESSIVE LOOP CLOSURE AND PITCH SENSOR 



1UXWZ SpMdOjpCowt ( t H^l. jwlod. in SPQD.OP «** ) 
TUUMZ STOTECoaat < Counter far • nrtm .tor*i ) 
U«UMZ ISTCM ( LOT •cc.l.i.ti.m comb* .ies« ) 

••»»•"»•» T/rr — w ' . — , 

■ — v , ro . iaw;0« > 

filllBIZ UTtU ( LflT tAchoMUr ccmpmxm*tar .tot*-10o3 ) 

VAJLLUSUS Tt*CHJ ( TocboaotOX MWmWBt •lOoi ) 

TUIABU Tocc* ( JLcc«l«n M t«r ■mww— rt • 10* ) 
VARIABLE Q*3 ( tfh~l Torqoo •lOO ) 



400 1000 BldOHOTD SLATE 
400 1000 TOMIIAL BOSS 

C SEAL TDK CL0CE REFIOaUmna TODS 

COT KTC^t ( Sits Bool Tloo Clock CoanUr 1 to m^I. „ 
IBVE.L DO, -(16) < »mt cantou of 00 on stack ) 



WTE.B tf74 > DCctrUll» ( Bowrif. Ctrlsoxd far cotmtor 1 ) 

•tTCDariodl.DO 
KTE.B DO.KTCcotMTUrl ( Sond low byto ) 
LSt-W tt.DO 

1012.8 DO.BCcorotoxl C Sond high byto ) 

■31B.L 06)4,00 ( gootaroo ori^iaol fslnt of 00 ) 
KB ( Imn to oohrovtiAo ) 

ESD-C0CE 

CR .( Loodod SXCoot ) 



ALSO HATH 
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5P32 

mniu sinm nn^ ^ ?UX0T ( ^ ^ 

PVABUBU 3TQBE2 2?S2Lc~rtl. ->-. t.r rm . , 



Z*»th.nciS < - ) ( th. tick amnt« r«i,ur ) 

CULL (IS) 

RTS 

EUXODE 

C» . ( Lo*d#d ZEBOtfctTlCIS > 

co« ncotosria c - » , ( tlek comt „ ^ rf ^ . 

■W.I OS).-(A6) ( c™t tick coro ^ _> ^ , 

am-coce 

CR . C Loadod TldStoSTlCI ) 



coram, mini iiBBKcrsH b bbb 

3 2048 UIMCABOR >! 30«8 LCTMCUM ?! ; ( Quick 



) 



aotora atop ) 



7MCS.TOME '■"•) ( Chock il OJC com 5.t«af 



OOP 4096 >( ch«* « or po^tiT, «tai»tion ) 
IP DBBP 4096 ( deep largo vain* ft oopply sat. nlaa ) 

naroo< (ehetl(Itlll(itlWltt|iitlw) 

IF oaop 0 < drop laxgo t»1o# • m . ^ , 

™n TBI , ( oxbax^. k-n.-^Huc ^ > 



: K4TT« ( a - ) C 3/W MIT f or nOO 
A DO tlMP ; 



TICCtoSTlCX C g«t tick cowt ) 

« > : < aota tro. Oag i* <tt >- Toaaplo ) 

CR .< Loaded CBECITSUPLE ) 



P.J. SUCCESSIVE LOOP CLOSURE AND PITCH SENSOR 



: Wtt ACTTTATK ( AcUt*i* Background task ) 
S C Step all aotora ) 

0 IfiTCad ! ( Initial LCT rof oronco inpot u««d - 0 ) 

0 lCT«v i ( Initial^ t^=b. r-~>. »tet<i to zaro ) 
0 LCT=ak f ( Iaitializa acc. coop, stats to zaxo ) 

0 S p oo dOfrCuunt ! ( Initialixa SPEED.OP comttar ) 

0 STOBECotmt f ( TirttlallTO STUCi amy sntry conntor) 
KTCoat ( Sot Boal Tiao clock to Tsanplo ) 

BBGZI 

CBBCXXSAflFLE ( Chock if saapla tiao baa srpdrsd ) 

IF ZnOtaoTICIS ( If THE Booot cotmtsr to zaxo . . . ) 

( •••»*•••••••• B£io lot ticanazi aid accn aagm •**« 

UrreiCADW m < road binary valoo froa poo «ac ) 
( &t <- top of stack ) 

LCTACOBX lDUDOl ¥! < switch, BOI to LCT accslaroaotar ) 

1 Saflim V! ( aaaplo analog signal ) 

1 tfAITU ( wait 30 alczoooc for Safi sottlo ) 

0 SftflAXXft ?! ( placa Skfl drrlcs in B3XJD sod* ) 

1 ADCAHft V! ( Start* A to 0 Coafoxtar ) 
1 EAIT4J ( omit for ADC to finish ) 

ADC4D0B. M ( roada binary tains froa A/D ) 
( Bt Ba <- top of stack ) 

4096 - 4884 • 10003660 ♦ ( coavort to a ToltagooiOofi ) 
2043S 1000000 a/ ( seals by LGTACCGAH -> acc*10o6 ) 
( Bt Ta6 <- top of stack ) 
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R3PT^! (lme0pyef|Calte!riat) 
Ot Trf <- top of s t*ck ) 



SpoodDpCouot • 0 > ( 

locp caffnsiTXDi < 



IF C if so . «dd tccilcnutcr cobbm.***. ) 



if spoodn? pbsso omr ) 

( .> 



t^C** • ( «st sccolsrmtfcm coo*** ) 
5 10O •/ ( »it. bj tr to 10^ . MM , 

W - ( «*tnct Too to got sec. czrarciO* > 
K* ( sm s ^ for co^»Ator nprfsto lstox ) 
( Bt Et6 E»6 <- top of st «ck ) 



****** < «« UT •cc^«o^r cospos^tor .uto > 
( »t Et£ Eftfi J*6<- top of .tsck ) 



8 100 •/ ♦ ( snltiply by 



^ to sec. «or ) 



1000 •/ ( s^t. by tCTls. by 1000 -> d..K>s3 > 

( Bt E*£ 4*6 <- tc? of stock > 

D80P ( drop r«6 txxm top of stock ) 
0 0 < supply zsxos f or Es£ sal d*3 ) 
( Bt 0 0 <- top of stock ) 



LOOP OQ&FEBATIGf 
THEI ( do tscbosjotsr cospsnsstiaa loop ) 

°"«8* 0 ♦ ( sbool spsod ♦ <U3 ) 

C Bt EsS sib <• top of stock ) 



( got bi-szy tsch rsodiag ) 



P.J. SUCCESSIVE LOOP CLOSURE AND PITCH SENSOR 



( Ea6 sua 9t <- top of stock ) 

HP ( ask* a copy to ) 

< dstorai&s *iga Utox ) 

10471S76 STOP / ( g* t ob*(7tftca>10«3 ) 

STOP 2 WD O ( gtt of tfcs thnl spnd : ) 
I? ECiTE THE! ( 0 *> crsn t *> nog. sposd ) 
( E*6 aim Tt3 \* top of stack > 

DO* Ttach3 • ( uti * copy to print Utor ) 
( Eo6 soa Tt3 <- top of stock ) 

( gat Tolocity «rror«10o3 ) 
- ( Eo6 Et3 <- top of stock > 

SCT C S«»« copy for toco coop txpdsto > 
( E*6 Et3 Et3<- top of stock ) 

i-GTrtk • ( Ctt LCT tock coapsasotor stoto ) 

( Es6 Et3 Et3 ItK- top of stock ) 

2S 100 •/ ♦ ( aaltiply by pt-rt sad ) 
( odd toch error } 

LOTt • ( ssltiply with toca loop gala to ) 

( Shool torqao*10o3 ) 

( Eo£ Et3 Qs3 <- top of stock ) 

0*3 ! ( saks « copy to prist latox ) 
( Estf Et6 Qs3<- top of stock > 

15120 ♦ 2048 15120 •/ ( got biaary coaaaad for D1C ) 
TD1CSATUE1TE ( chock if DiC satazato* ) 
LGTDACADDR V! ( output torqno coaaaad ) 



352 



APPENDIX P. FORTH CODES FOR LONGITUDINAL CONTROL 

' Elf Et3<- top of stock ) 



COiraSATMl STATE QFDATE 



ICTxtk * ( gt LCT tach cooootss .tots ) 
( E«6 Et3 Xt3 <- top of stock ) 

100 100 •/ C sultiply by LCTpt ) 

- Unxtk • ( odd to too tsch. orror sod so,* xbotClfl) j 
C Eo6 <- top of stock ) 



WTtakt ( ^rt LCT sccolorosstor cospsasotor stats ) 



< Ea6 1*6 <- top of stock ) 
103 100 •/ ( soltiply by LCTfcs ) 

t Untak C odd to too sec. error sod uti xhsttt+i) ) 
( »tock sspty <- top of stock > 



1 SpoodObCooat ( 



V STOXE TDK BISTORT 

STOHECormt < STDKELsogUfc < 

IF 

Q«3 • I>F F/ ( Scolo tock to Doroml ) 

STORE! STOffiComrt t 2* 2* + F! C sultiply ^ ) 

( 4 to stars 64 hit t ) 

Ytocb3 • X>r 1000. F/ ( Scolo bock to ooroml ) 
STDBE2 STUBEsoBt i 2s > ♦ F! 



Tocc6 « I>F 1000000. F/ ( Scolo bock to norml ) 
STOHE3 STORECoaat t 2* 2* ♦ F« 

1 STOHECoutt ♦! ( Incrasnt t stoxod ) 



P.3. SUCCESSIVE LOOP CLOSURE AND PITCH SENSOR 
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THE! P1D5E ( Go do tmiml fak ) 

( — SOU-DUG OF TOTOIAL t BACIC8OTD TASKS FM TO8HET APPLICATIOI •« ) 

: WHIP ( Baild mad UxXrmtm Badgrorad tuk > 
SU1E BOttD 
SUTE TORI ; 

: 7XET ( Cbocl il toy m ckpntMd ) 

BSHI 

PAUSE 

OT 

IF 

IET DROP 
THEI 

UIIJLL ; 

: PRUT ACTIVATE < Actitmto TcrKUial t*o* ) 

•1 HIT-!MSE ( rrrrt -11 ) 

CI 

h«» oyteytOMt BJCKGBOQID rxriafalo - 
7IET 

STOHEXttgth 0 DO 

STDBE1 I 2«2*>FI 

I 8 HOD 0- IF " CR 

THEI F. • 

PAD5E LOOP 3; » Cft 
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CK Tt*efc-C - 
CTDRELcBgtfc 0 00 

ST0HE2 I 2* 2f «rt 

I 8 HDD O* IP .* .. - ci 

TBSM T. , J - 

P105S ICS? .* J; • £R 

CR Tccc-C w 

STDBELagth 0 00 

ST0RE3 I > > ♦ pf 
I 8 no 0* rF ■ CE 

TBEI F. .« - 

paose loop j; - ca 



agah : 

: START 

CHADEX* BOSS BOM) ( InitUUx,. T-wial T»k ) 
BOSS PHUT 

WHIP : ( InitULU*. B^roiad Tmmk ) 



TOWET START ( Ttzni progrm. into . TURHET «pplic*ti« ) 

cr ca ci 

( «rit« protect STATIC mmory «nd met tte computer ) 



EOF 



Appendix Q 



Lateral Control System 



Q.l Lateral equations of motion 

The lateral equations of motion can be found from derivations in Appendices A, B 
ad C. The lateral equations can be decoupled from the longitudinal equations of 
motion under the same conditions mentioned in section N.l. 



Equations A.90, A-91 and A-92 then simplify to: 

I*? + If& = -l?Sl<j - fci + /jv-Qt 



(Q-D 



+ if + If + mwiir + ntfirw + r F ) 2 + m^r^r + rr) 2 ]^ 
= [I? + mwrw + mprwirw + r F ) + m-rrw^ + fT)]iW 
+ [mwrw+m F irw+r F ) + m T {r w + r T )]g4> (Q^) 



(Q-3) 



If the d.c. motor rotor inertia is non zero the additional terms due to the geared 
drive system that should be included in the lateral dynamic equations of motion 
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can be determined by inspection of eqnation. G.13 and G.14 in Appendix G 
{% + /f + (1 - n)I*$ + tt(1 _ n)I ^ 
= -FQoi-fci + fTi-QT (Q4) 

: =lfg r [/J <- . - - + <y r (Q 5) 

Equation. Q.2. Q.4 ,nd are the dynamic equation, of motion of the lateral 
system, where /« is the mcment of inertia of the rotor of the turnt-_ble drive motor 
and n b the gear ratio of the turntable drive system. 



Rewriting these equations into state 



Where: 



space representation, they become: 



0 


Aj 


A3 


0 












It 


0 




4 ' 




-1 




Al 


0 


0 


0 




+ 




0 




0 


Ju 




j> 




0 




0 


/» 


/» 


0 








0 


0 


-/r 


0 




1 


+ 


1 


Qt 


0 


0 


0 


1 








1 


0 


0 


0 








0 





(Q.6) 



A 2 
A 3 
Al 



* + * + (Q.7) 

(Q-8) 

1 + A + A + mHrrJ, + TOf {rwr + r F ) 2 + + r T ) 2 

(Q.10) 

M (Q-n, 

. (Q-12) 
[1? + m«rr=, + mrrwir* + r F ) + m r r w ( nv + (Q.i 3 ) 
[m^r^ + mpirw + r F ) + m T (r,e + ^ (Q 14) 

Multipucation of equation Q.6 by the inverse of the first matrix containing the 
inertia terms, yields the standard state space form of the system of lateral dynamic 



/» 
/a 
Ai 

J22 
Jl4 



Q-Z LATERAL SENSORS 
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equations: 

x = Fx + G« (Q15) 
whe~ * = Q T ^z x = ft * 9 

Q.2 Lateral sensors 

A tachometer anJ an accderorwter are used as the lateral season. A roll angle 
sensor, for example a vertical gyro, is also considered as an alternative to the ac- 
celerometer. 

The tachometer's operation is similar to that of the one in the longitudinal system, 
but here it is mounted on the rotor shaft of the turntable drive motor. It reads 

"LAT = niAT • V (Q.16) 

where nur is the known gear ratio of the turntable drive system. The micropro- 
cessor can scale the tachometer measurement so that the state r? is measured: 

* = 1 (Q.17) 

The accelerometer is mounted on the unicyde frame so that its acceleration sensi- 
tive axis is in the direction of the sideways motion of the unicyde It is placed on 
the vertical centeriine of the unicyde, so that it will not measure the component 
due to yaw acceleration. 



The accelerometer is actually a pendulum with a servo feedback loop around it 
to change it to an accelerometer, as described in Appendix H. The output of the 
instrument is a signal proportional to an internal control torque This is generated 
to counter the effects of specific forces on the unicyde frame at the position where 
the accelerometer is mounted. The component of the frame sideways acceleration 
can be obtained from equation A.76 and the acceleration due to gravity is -g<t> for 
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smau roil angles. Figure Q.i snow. now the sign of the accelerometer measurement 
is determined. 



<?p(t4n = M-(nv+r S3 )^ + r K ra 0 ^ + ^j (Ql8) 

where is the height above the waeel axle at which the accelerometer is mounted. 
From equation Q.I5 

* = Fn* + Fi 2 i + F l3 rt + F u <fi + GxQ T ^ ir> 

By scaling the accelerometer measurement in the microprocessor software and by 
substituting equation, Q.19 into Q.18, the lateral acceleration measurement can 
be rewritten in terms of the states and control input: 

V- = -(. r W*-r a )F u ^ + [r w a Q -(r w + r 53 )F l 2]i> 

- ( r » r +'-S3m3i? + [y-(rHr + rs 3 )F 1 ^-(r M r + r S3 X? 1 <3 T 

(Q-20) 

The measurement obtained from an ideal rofl angle sensor would be 

Q.3 Lateral system characteristics 

The lateral system dynamic behaviour is a function of the nominal wheel speed 
Ho, as can be seen from equations Q.12 and Q.13. The results presented in sec- 
tion Q-3.1 show the state transition input and output matrices lor a typical wheel 
speed of 3 rad/sec The measured mechanical parameters used in these calcula- 
tions a~ listed in Appendix M. 



QJ. LATERAL SYSTEM CHARACTERISTICS 

Q.3.1 Lateral system characteristics calculation 



acTci2/uitauLcni 

LATOAL SYSTEX CHUUC7Z8X31IC3 

LITERAL STATES : PHI. DOT; PSI.DCT; ETA; PHI 

coram, hot : turstabie urnm idique <qt> 

""^^SWS : TACHDHOTi; ACCHBttHEIZI; BOIL UCLE SE1301 
OUTS : FCTERS. RADIUS. SBCMM 



KMHA1 JHEZL SP£H) (2AD/SEO : 
QRECAO 

3.0 

iCCELZgMB ni ebxqtt CHETEB) : 
tS3 

G.660O 

OPEILDQP ST3TEH HATBICES: 



PUT 



360 



APPENDIX Q. LATERAL CONTROL SYSTEM 



0. 0.6575 0. 10.9719 

-C.2240 -0.0492 0.1212 0. 

0.1718 0.0378 -0.2122 0. 

1.0000 0. o. 0. 



CUT 



0. 
-1.5415 
2.6999 
0. 



HUT 



0. 
0. 
0. 



0. 

0.0294 

0. 



1.0000 

0. 

0. 



0. 

0.5387 
1.0000 



IUT 



3. 
0. 
0. 



DGV1L 

-O.02S0 
-0.2384 
-3.2890 
3.2910 



EIC7EC m 

0.0015 -0.0094 

1.0000 -0.6515 

0.2031 1.0000 



1 0000 1.0000 
0.0713 -0.0653 
-0.0S67 0.0483 
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-0.0699 0.0392 -0 3040 0.3039 
era m 

0.1828 

0.1673 
-0.142S 



08S • 

0.2031 1.0000 -0.0667 0.0483 

-0.0028 0.0020 -0.1617 0.1618 

-0.0699 0.0392 -0.3040 0.3039 

RES IDC • 

0.0371 2.6792 -0.0096 -O.0069 

-0.0006 O.0052 -0.0271 -0.0231 

-0.0110 0.1061 -O.0609 -0.0433 

TR1BPER Pt»cnOf FRflH TDBJTiBtE COTE TTHUJOE TO TICHQKIDL 

TACBZEB03 - 



3.3000 
-3.2998 
-0.0279 



3.2910 
-3.2890 
-0.2384 
-0.0250 



T1CBC1II 
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2-6999 

tbustzr nacnoi f**i tout*^ am. TOBQ0E ro .annaacia nisoraar 

AOCZEBOS • 

0.0000 ♦ 1.03841 
0.0000 - 1.0284i 
0.0000 - i. i 

ACCPOLES • 

3.2910 
-3.2890 
-0.2384 
-0.0250 

ACCGAH 

-0.0454 

trabfei Ftacntn rant turitabiz mcte tqkjoe to wll akue HEAscsoor 

HnTT7FWW • 
0. 

HOIXPOLES • 

3.2910 
-3.2890 
-0.2384 
-O.02S0 

R0LLCAH • 
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-1.0135 



Q.4 LQG control system design 

la this section we will design aa optimal continuous time linear quadratic gaussian 
compensator for the lateral system. 

The unicyde yaw rate will accurately follow a reference command if the regulator 
uses integral error feedback of the measured plant yaw rate. This introduces an 
extra state, c, to the lateral system matrices: 



t 




0 H 




t 




0 




-1 












+ 








X 




0 F 




X 




G 




0 



tt = 



= -IC. C] 



where H = ( 0 1 0 0] 



(Q-22) 



The regular gains C, for aa optimal linear quadratic regulator were calculated by 
minimizing the cost function: 



(Q23) 



A CTfiX-C program Tgtlqgxtr' was used to calculate the regulator gains and the 
print-out is shown in section Q.4.1. The ratio f of the weighting factors were 
chosen so that the step response (Figure Q.2) of the closed loop system reached 
the commanded value in approximately 5 seconds. Figure Q-3 shows the closed 
loop frequency of the lateral system. The closed loop system has a bandwidth of 
approximately 0 J Hz, which is comparable to that of the longitudinal system. 
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A linear quadratic gausoion estimator was designed to estimate aO the lateral states 
W.hetachometerandroflangtese^ Tie modified plant model 

that includes the random disturbance effect, on the plant and measurements 



i = Fx + Gu + G n w 

Vm. = Afx + V 

where: 

w is a random disturbance vector with spectral density Q 
y m is a vector of the measured quantities 
r is a ra=dc«Ti measurement noise vector 
G n is the process noise input distribution 



matrix 



The complete statistical natnre of the measoremeat noise is not known because 
the actual sensor, have not been selected. For this reason we will assume the 
same spectral densities for the tachometer (ft) and roll angie sensor (ft) „ for 
the longitudinal system (refer to section fU). 

ft = 0.05 rod J /s 

ft = 0.1 mV (q.25) 

The process aois* spectral density is also asmmed to be similar to that of the 
longitudinal system. Tne no»e input dirfribution matrix <7 n is assumed to be the 
same as the control input distribution matrix 

Q = 2 N 7 m 2 s 
°* = G (Q-26) 
A Kalman-Bucy filter (Kalman] with filter gains L, can be designed to provide an 
optimal estimate of the state vector i in the presence of the specified disturbance 
inputs. The estimator state equation is 

x = /ox + GoU + £(, m -A/x) (Q^-) 
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\M? the plant used in the estimator would have the same parameter, as 

the actual plant, i.e. F 0 = F and G 0 = (7. 



The Kalman filter gain matrix L is shown under ESTGAINS in the print-out listed 
in section Q.4.1. Eqoations Q.22 and 0.24 can be combined to give the lateral 
closed loop system matrices: 



F -GC t -GC 
0 0 S 
LM -G 0 C C Fo-GaC-LM 





X 




0 






e 


+ 


-1 


r+ 




z 




0 





G n 0 
0 0 
0 I 



(Q.28) 

The performance of the state estimator is simulated where the actual initial roll 
aagle #0) = 2 degrees, while the estimated roll angle is #0) = 0. Figure Q.4 
show, that the rofl error initially increases to approximately 2.4 degrees while the 
state estimates are improved. In the absence of noise the estimated state, track 
the actual plant stages and the rofl error is reduced to zero in approximately 5 
seconds. 



Q.4.1 Calculation of LQG gains 



WTClZOATLQa.cn 

LATERAL COTTB0L STSTEH KITH PULL QMDSX U) ESTdUTDE 



UIEHAL ST1IE5 : PHI. DOT; PSI.DOT; ETA; PHI 
COTTBOL HKTT : TD8JTTABLE H3TDR TOHJUE «JT) 
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KiSDHDESn : TlCHDHOTU BOIL AKt£ 3H30R 
UlTIS : HE7SRS, EUDZUS, SECOP g 

•—^ cottuddis tub puit iutriczs 

FUT 

0.<KX>0 0.6S7S 0.0000 10.9719 

-0.2240 -0.0492 0.1212 0.0000 

0.1718 0.0378 -0.2122 0.0000 

1.0000 O.OOOQ 0.0000 0.0000 

GUT 



0.0000 
-1.5415 
2.6999 
0.0000 

0.0000 
-1.5415 
2.6999 
0.0000 

HUT • 

0-0 0.0 i.o 0.0 
0.0 0.0 0.0 1.0 

IUT 



0.0 
0.0 
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***** iurxces for otbeux nan ras B t a coram mu 

EnEttED STATE TECTO* D : PSHWT.EU.; PHI. DOT; P3I.D0T; ET1; 
FUT 



0.0000 


0.0000 


l.OOOO 


0.0000 


0.0000 


0.0000 


0.0000 


0.6575 


0.0000 


10.9719 


0.0000 


-0.2240 


-0.0452 


0.1212 


0.0000 


0.0000 


0.1718 


0.0378 


-0.2122 


0.0000 


0.0000 


1.0000 


0.0000 


0.0000 


0.0000 



cur • 

0.0000 
0.0000 
-1.641S 
2.6999 
O.OOCO 

•***• REGOLATDft SE31GM ••••• 

cost rrmcmw hexchitbc factdes oi states aid coitbol : 

ADIAG 



6.0 0.0 0.0 0.0 0.0 



1.0 



QFTIHAI HTKXAI. ERROR UD STATE FEBOBACI CAOS 



2.2361 
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-46.4187 -5.9S71 -0.0787 -150.9263 
RECGU1 

-3.4468 
Mi., T^ft m 

-3.3124 
3.3124 

-o.ocoo 

RECP0LE3 • 

-0.0000 ♦ 0.00001 
-1.3237 ♦ 1.31801 
-1.3237 - 1.31801 
-3.2921 - 0.00641 
-3.2921 ♦ 0.0064i 



***** ESTDUTDa oesici ***** 

PROCESS UD HEIsrair USE SPECISiL aasmEs ; 
OUT 

1.0 

RUT 



0.0500 
0.0000 



0.0000 
0.1000 



Q 4. LQG CONTROL SYSTEM DESIGN 

GPTIHAL ESTDUTOa Q1H5 $MD ESTDUTDt POLES ; 
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ESTGAIT5 

12390 21. 7228 

11.866? 0.2314 
0.4628 6.6832 

ESI POLES ■ 

-O.0279 ♦ O.OOOOi 
-3.3009 ♦ 0.04171 
-3.3009 - 0.04171 
-12.07.10 - 0.00001 



APPENDIX Q. LATERAL CONTROL SYSTEM 
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if no control torque 
had been applied 
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frame acceleration 

for lateral acceleration 
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pendulum control torque 
Q p (LAT) > 0 



Figure Q.l: Lateral Arcelerometer Measurement 
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Figure Step Response of the Lateral System with Full State Feedback 
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